Merge pull request 'MEKF Fixes' (#843) from mekf-fix into main

Reviewed-on: #843
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -30,6 +30,7 @@ will consitute of a breaking change warranting a new major release:
   and is triggered by the `AcsController` now.
 - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
 - Fixed calculation of desaturation torque for faulty RWs.
+- Fixed bugs within the `MEKF` and simplified the code.
 
 ## Changed
 
@@ -50,6 +51,8 @@ will consitute of a breaking change warranting a new major release:
 
 - Updated STR handler to unlock and allow using the secondary firmware slot.
 - STR handling for new BlobStats TM set.
+- Added new action command to update the standard deviations within the `MEKF` from the
+  `AcsParameters`.
 
 # [v7.6.1] 2024-02-05
 

mission/controller/AcsController.cpp

@@ -6,6 +6,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
       sdcMan(sdcMan),
       attitudeEstimation(&acsParameters),
       fusedRotationEstimation(&acsParameters),
+      navigation(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       ptgCtrl(&acsParameters),
@@ -63,7 +64,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
     }
     case UPDATE_TLE: {
       if (size != 69 * 2) {
-        return INVALID_PARAMETERS;
+        return HasActionsIF::INVALID_PARAMETERS;
       }
       ReturnValue_t result = updateTle(data, data + 69, false);
       if (result != returnvalue::OK) {
@@ -84,8 +85,11 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
       }
       std::memcpy(tle + 69, line2, 69);
       actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
-      return EXECUTION_FINISHED;
+      return HasActionsIF::EXECUTION_FINISHED;
     }
+    case (UPDATE_MEKF_STANDARD_DEVIATIONS):
+      navigation.updateMekfStandardDeviations(&acsParameters);
+      return HasActionsIF::EXECUTION_FINISHED;
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -176,7 +180,7 @@ void AcsController::performAttitudeControl() {
       mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
       &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &attitudeEstimationData, &acsParameters);
+                              &susDataProcessed, timeDelta, &attitudeEstimationData);
 
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {

mission/controller/AcsController.h

@@ -113,6 +113,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
   static const DeviceCommandId_t UPDATE_TLE = 0x3;
   static const DeviceCommandId_t READ_TLE = 0x4;
+  static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
 
   ReturnValue_t initialize() override;
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -1,14 +1,16 @@
 #ifndef MULTIPLICATIVEKALMANFILTER_H_
 #define MULTIPLICATIVEKALMANFILTER_H_
 
-#include <stdint.h>
-
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "AcsParameters.h"
-#include "eive/resultClassIds.h"
+#include <common/config/eive/resultClassIds.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class MultiplicativeKalmanFilter {
-  /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
+  /* @brief: This class handles the calculation of an estimated quaternion and the gyroscope bias by
    *           means of the spacecraft attitude sensors
    *
    * @note: A description of the used algorithms can be found in the bachelor thesis of Robin
@@ -18,56 +20,26 @@ class MultiplicativeKalmanFilter {
  public:
   /* @brief: Constructor
    */
-  MultiplicativeKalmanFilter();
+  MultiplicativeKalmanFilter(AcsParameters *acsParameters);
   virtual ~MultiplicativeKalmanFilter();
 
-  ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
+  ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  /* @brief: init() - This function initializes the Kalman Filter and will provide the first
-   * quaternion through the QUEST algorithm
-   * @param:  magneticField_  magnetic field vector in the body frame
-   * 		  sunDir_ 		  sun direction vector in the body frame
-   * 		  sunDirJ 		  sun direction vector in the ECI frame
-   * 		  magFieldJ 	  magnetic field vector in the ECI frame
-   */
-  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
-                     const bool validSS, const double *sunDirJ, const bool validSSModel,
-                     const double *magFieldJ, const bool validMagModel,
-                     acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
+  ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
+                     const acsctrl::MgmDataProcessed *mgmData,
+                     const acsctrl::GyrDataProcessed *gyrData,
+                     acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
-   * for the current step after the initalization
-   * @param:  quaternionSTR   Star Tracker Quaternion between from body to ECI frame
-   * 			rateGYRs_ 		Estimated satellite rotation rate from the
-   * Gyroscopes [rad/s] magneticField_  magnetic field vector in the body frame sunDir_
-   * sun direction vector in the body frame sunDirJ 		sun direction vector in the ECI
-   * frame magFieldJ 		magnetic field vector in the ECI frame
-   * 			outputQuat 		Stores the calculated quaternion
-   * 			outputSatRate 	Stores the adjusted satellite rate
-   * @return 	ReturnValue_t	Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
-   * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
-   * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
-   * 							RETURN_OK else
-   */
-  ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
-                        const bool validGYRs_, const double *magneticField_,
-                        const bool validMagField_, const double *sunDir_, const bool validSS,
-                        const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
-                        AcsParameters *acsParameters);
+  ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
+                        const acsctrl::MgmDataProcessed *mgmData,
+                        const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
+                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
 
-  enum MekfStatus : uint8_t {
-    UNINITIALIZED = 0,
-    NO_GYR_DATA = 1,
-    NO_MODEL_VECTORS = 2,
-    NO_SUS_MGM_STR_DATA = 3,
-    COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
-    INITIALIZED = 10,
-    RUNNING = 11,
-  };
+  void updateStandardDeviations(const AcsParameters *acsParameters);
+
+  void setStrData(const double qX, const double qY, const double qZ, const double qW,
+                  const bool valid);
 
-  // resetting Mekf
   static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
   static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
   static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
@@ -82,26 +54,93 @@ class MultiplicativeKalmanFilter {
  private:
   static constexpr double ZERO_VEC3[3] = {0, 0, 0};
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
-
-  /*Parameters*/
-  double quaternion_STR_SB[4];
-
-  /*States*/
-  double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
-  double initialCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+  static constexpr double ZERO_MAT66[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                               {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                               {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
-  double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
-  uint8_t sensorsAvail = 0;
+  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
+  static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
+                                        {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
 
-  /*Outputs*/
-  double quatBJ[4];  /* Output Quaternion */
-  double biasGYR[3]; /*Between measured and estimated sat Rate*/
-                     /*Parameter INIT*/
-  /*Functions*/
-  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
-  void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
-                     double quat[4], double satRotRate[3]);
+  enum MekfStatus : uint8_t {
+    UNINITIALIZED = 0,
+    NO_GYR_DATA = 1,
+    NO_MODEL_VECTORS = 2,
+    NO_SUS_MGM_STR_DATA = 3,
+    COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
+    INITIALIZED = 10,
+    RUNNING = 11,
+  };
+
+  enum SensorAvailability : uint8_t {
+    NONE = 0,
+    SUS_MGM_STR = 1,
+    SUS_MGM = 2,
+    SUS_STR = 3,
+    MGM_STR = 4,
+    SUS = 5,
+    MGM = 6,
+    STR = 7,
+  };
+
+  MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
+
+  struct StrData {
+    struct StrQuat {
+      double value[4] = {0, 0, 0, 0};
+      bool valid = false;
+    } strQuat;
+  } strData;
+
+  // Standard Deviations
+  double sigmaSus = 0;
+  double sigmaMgm = 0;
+  double sigmaStr = 0;
+  double sigmaGyr = 0;
+  // sigmaV
+  double sigmaGyrArw = 0;
+  // sigmaU
+  double sigmaGyrBs = 0;
+
+  // Covariance Matrices
+  double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+  double covAposteriori[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+
+  // Sensor Availability
+  SensorAvailability sensorsAvailable = SensorAvailability::NONE;
+  uint8_t matrixDimensionFactor = 0;
+
+  // Estimated States
+  double estimatedQuaternionBI[4] = {0, 0, 0, 1};
+  double estimatedBiasGyr[3] = {0, 0, 0};
+  double estimatedRotRate[3] = {0, 0, 0};
+  double estimatedCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+
+  // Functions
+  ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
+                                      const acsctrl::MgmDataProcessed *mgmData,
+                                      const acsctrl::GyrDataProcessed *gyrData,
+                                      acsctrl::AttitudeEstimationData *attitudeEstimationData);
+
+  void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
+                double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
+  ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
+                       acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
+  void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
+  void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
+
+  ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+
+  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
 };
 
 #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

mission/controller/acs/Navigation.cpp

@@ -1,40 +1,27 @@
 #include "Navigation.h"
 
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>
-
-Navigation::Navigation() {}
+Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
 
 Navigation::~Navigation() {}
 
-ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
-                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
-                                  acsctrl::SusDataProcessed *susDataProcessed,
-                                  acsctrl::AttitudeEstimationData *mekfData,
-                                  AcsParameters *acsParameters) {
-  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
-                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
+ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
+                                  const acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                  const acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                  const acsctrl::SusDataProcessed *susDataProcessed,
+                                  const double timeDelta,
+                                  acsctrl::AttitudeEstimationData *attitudeEstimationData) {
+  multiplicativeKalmanFilter.setStrData(
+      sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
+      sensorValues->strSet.caliQx.isValid());
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
-    mekfStatus = multiplicativeKalmanFilter.init(
-        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
-        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
-        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData,
-        acsParameters);
+    mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
+                                                 gyrDataProcessed, attitudeEstimationData);
     return mekfStatus;
   } else {
     mekfStatus = multiplicativeKalmanFilter.mekfEst(
-        quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
-        gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
-        mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
-        susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
-        susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(), mekfData, acsParameters);
+        susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
     return mekfStatus;
   }
 }
@@ -79,3 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
 ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
   return sgp4Propagator.initialize(line1, line2);
 }
+
+void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
+  multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
+}

mission/controller/acs/Navigation.h

@@ -5,24 +5,24 @@
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
-#include <mission/controller/acs/SensorProcessing.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class Navigation {
  public:
-  Navigation();
+  Navigation(AcsParameters *acsParameters);
   virtual ~Navigation();
 
-  ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
-                        acsctrl::GyrDataProcessed *gyrDataProcessed,
-                        acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        acsctrl::SusDataProcessed *susDataProcessed,
-                        acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
+  ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
+                        const acsctrl::GyrDataProcessed *gyrDataProcessed,
+                        const acsctrl::MgmDataProcessed *mgmDataProcessed,
+                        const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
+                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
   void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
   ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
+  void updateMekfStandardDeviations(const AcsParameters *acsParameters);
 
  protected:
  private: