mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -27,7 +27,7 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
   kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 
-void MultiplicativeKalmanFilter::init(
+ReturnValue_t MultiplicativeKalmanFilter::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::MekfData *mekfData) {  // valids for "model measurements"?
@@ -189,10 +189,12 @@ void MultiplicativeKalmanFilter::init(
     initialCovarianceMatrix[5][4] = initGyroCov[2][1];
     initialCovarianceMatrix[5][5] = initGyroCov[2][2];
     updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
+    return KALMAN_INITIALIZED;
   } else {
     // no initialisation possible, no valid measurements
     validInit = false;
     updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
+    return KALMAN_UNINITIALIZED;
   }
 }
 
@@ -209,12 +211,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
     updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
-    return KALMAN_NO_GYR_MEAS;
+    return KALMAN_NO_GYR_DATA;
   }
   // Check for Model Calculations
   else if (!validSSModel || !validMagModel) {
     updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
-    return KALMAN_NO_MODEL;
+    return KALMAN_NO_MODEL_VECTORS;
   }
   // Check Measurements available from SS, MAG, STR
   if (validSS && validMagField_ && validSTR_) {
@@ -852,7 +854,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
   if (inversionFailed) {
     updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
-    return KALMAN_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
+    return KALMAN_COVARIANCE_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
   }
 
   // [K = P * H' / (H * P * H' + R)]
@@ -1083,10 +1085,17 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
-  return returnvalue::OK;
+  return KALMAN_RUNNING;
 }
 
-void MultiplicativeKalmanFilter::reset() {}
+void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
+  double resetQuaternion[4] = {0, 0, 0, 1};
+  double resetCovarianceMatrix[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}};
+  std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
+  std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
+  updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
+}
 
 void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
                                                          MekfStatus mekfStatus) {

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -15,8 +15,7 @@
 #ifndef MULTIPLICATIVEKALMANFILTER_H_
 #define MULTIPLICATIVEKALMANFILTER_H_
 
-#include <stdint.h>  //uint8_t
+#include <stdint.h>
-#include <time.h>    /*purpose, timeval ?*/
 
 #include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
@@ -30,7 +29,7 @@ class MultiplicativeKalmanFilter {
   MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
   virtual ~MultiplicativeKalmanFilter();
 
-  void reset();  // NOT YET DEFINED - should only reset all mekf variables
+  void reset(acsctrl::MekfData *mekfData);
 
   /* @brief: init() - This function initializes the Kalman Filter and will provide the first
    * quaternion through the QUEST algorithm
@@ -39,9 +38,10 @@ class MultiplicativeKalmanFilter {
    * 		  sunDirJ 		  sun direction vector in the ECI frame
    * 		  magFieldJ 	  magnetic field vector in the ECI frame
    */
-  void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
+  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
-            const bool validSS, const double *sunDirJ, const bool validSSModel,
+                     const bool validSS, const double *sunDirJ, const bool validSSModel,
-            const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData);
+                     const double *magFieldJ, const bool validMagModel,
+                     acsctrl::MekfData *mekfData);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
    * for the current step after the initalization
@@ -75,9 +75,14 @@ class MultiplicativeKalmanFilter {
 
   // resetting Mekf
   static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
-  static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
+  static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2);
-  static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2);
+  static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3);
-  static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3);
+  static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4);
+  static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5);
+  static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED =
+      returnvalue::makeCode(IF_KAL_ID, 6);
+  static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7);
+  static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8);
 
  private:
   /*Parameters*/