quest attitude estimation

mission/controller/acs/AttitudeEstimation.cpp

@@ -0,0 +1,91 @@
+#include "AttitudeEstimation.h"
+
+AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
+  acsParameters = acsParameters_;
+}
+
+AttitudeEstimation::~AttitudeEstimation() {}
+
+void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const double susI[3],
+                               const bool susIvalid, const double mgmB[3], const bool mgmBvalid,
+                               const double mgmI[3], const bool mgmIvalid, double qBI[4]) {
+  if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) {
+    // Normalize Data
+    double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
+           normSusI[3] = {0, 0, 0};
+    VectorOperations<double>::normalize(mgmB, normMgmB, 3);
+    VectorOperations<double>::normalize(mgmI, normMgmI, 3);
+    VectorOperations<double>::normalize(susB, normSusB, 3);
+    VectorOperations<double>::normalize(susI, normSusI, 3);
+
+    // Create Helper Vectors
+    double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
+           helperSum[3] = {0, 0, 0};
+    VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
+    VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
+    VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
+    VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
+    VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
+    VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
+
+    // Sensor Weights
+    double kSus = 0, kMgm = 0;
+    kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
+    kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
+
+    // Weighted Vectors
+    double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
+           kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
+    VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
+    VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
+    VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
+    VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
+    VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
+
+    // Some weird Angles
+    double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
+                       (VectorOperations<double>::dot(weightedSusB, normSusI) +
+                        VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
+                   VectorOperations<double>::dot(helperCross, kSumVec);
+    double beta = VectorOperations<double>::dot(helperSum, kSumVec);
+    double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
+
+    // I don't even know what this is supposed to be
+    double constPlus =
+        1. / (2 * std::sqrt(gamma * (gamma + alpha) *
+                            (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+    double constMinus =
+        1. / (2 * std::sqrt(gamma * (gamma - alpha) *
+                            (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+
+    // Calculate Quaternion
+    double qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, qRotVecPt1[3] = {0, 0, 0};
+    if (alpha >= 0) {
+      // Scalar Part
+      qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+      // Rotational Vector Part
+      VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
+      VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+      VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
+      VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+      std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+      VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+      QuaternionOperations::normalize(qBI, qBI);
+    } else {
+      // Scalar Part
+      qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+      // Rotational Vector Part
+      VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
+      VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+      VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
+      VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+      std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+      VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+      QuaternionOperations::normalize(qBI, qBI);
+    }
+  } else {
+    // ToDo: fill dataset and set to invalid
+  }
+}

mission/controller/acs/AttitudeEstimation.h

@@ -0,0 +1,28 @@
+#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
+#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
+
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+
+#include <cmath>
+#include <iostream>
+
+class AttitudeEstimation {
+ public:
+  AttitudeEstimation(AcsParameters *acsParameters_);
+  virtual ~AttitudeEstimation();
+  ;
+
+  void Quest(const double susB[3], const bool susBvalid, const double susI[3], const bool susIvalid,
+             const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid,
+             double qBI[4]);
+
+ protected:
+ private:
+  AcsParameters *acsParameters;
+
+  void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]);
+};
+
+#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

mission/controller/acs/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE AcsParameters.cpp
           ActuatorCmd.cpp
+          AttitudeEstimation.cpp
           FusedRotationEstimation.cpp
           Guidance.cpp
           Igrf13Model.cpp