MEKF cleanup

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -1,10 +1,3 @@
-/*
- * MultiplicativeKalmanFilter.cpp
- *
- *  Created on: 4 Feb 2022
- *      Author: rooob
- */
-
 #include "MultiplicativeKalmanFilter.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
@@ -31,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
 
 void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
-  kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/
+  kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 
 void MultiplicativeKalmanFilter::reset() {}
@@ -43,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
     validInit = true;
-    // AcsParameters mekfEstParams;
-    // loadAcsParameters(&mekfEstParams);
     //  QUEST ALGO -----------------------------------------------------------------------
     double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
     sigmaSun = kalmanFilterParameters->sensorNoiseSS;
     sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
-
+    sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
-    sigmaGyro = 0.1 * 3.141 / 180;  // acs parameters
 
     double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
            normSunJ[3] = {0, 0, 0};
@@ -138,7 +128,6 @@ void MultiplicativeKalmanFilter::init(
            matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
            matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
            matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    /* vector*transpose(vector)*/
     MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
     MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
     MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
@@ -201,8 +190,6 @@ void MultiplicativeKalmanFilter::init(
     initialCovarianceMatrix[5][3] = initGyroCov[2][0];
     initialCovarianceMatrix[5][4] = initGyroCov[2][1];
     initialCovarianceMatrix[5][5] = initGyroCov[2][2];
-    // initialCovarianceMatrix[][] = {{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}};
   } else {
     // no initialisation possible, no valid measurements
     validInit = false;
@@ -216,8 +203,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
     const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
     const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData) {
   // Check for GYR Measurements
-  // AcsParameters mekfEstParams;
-  // loadAcsParameters(&mekfEstParams);
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
     {
@@ -934,10 +919,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   double errStateQuat[3] = {errStateVec[0], errStateVec[1], errStateVec[2]};
   double errStateGyroBias[3] = {errStateVec[3], errStateVec[4], errStateVec[5]};
-  sif::debug << "errQ=[" << errStateQuat[0] << "," << errStateQuat[1] << "," << errStateQuat[2]
-             << "]" << std::endl;
-  sif::debug << "errW_bias=[" << errStateGyroBias[0] << "," << errStateGyroBias[1] << ","
-             << errStateGyroBias[2] << "]" << std::endl;
 
   // State Vector Elements
   double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
@@ -966,25 +947,18 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   biasGYR[2] = updatedGyroBias[2];
 
   /* ----------- PROPAGATION ----------*/
-  // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
+  double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
-  // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
+  double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
-  double sigmaU = 3 * 3.141 / 180 / 3600;
-  double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
 
   double discTimeMatrix[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}};
   double rotRateEst[3] = {0, 0, 0};
-  sif::debug << "MEKF rateGYR= " << rateGYRs_[0] << " " << rateGYRs_[1] << " " << rateGYRs_[2]
-             << std::endl;
-  sif::debug << "MEKF bias= " << updatedGyroBias[0] << " " << updatedGyroBias[1] << " "
-             << updatedGyroBias[2] << std::endl;
   VectorOperations<double>::subtract(rateGYRs_, updatedGyroBias, rotRateEst, 3);
   double normRotEst = VectorOperations<double>::norm(rotRateEst, 3);
   double crossRotEst[3][3] = {{0, -rotRateEst[2], rotRateEst[1]},
                               {rotRateEst[2], 0, -rotRateEst[0]},
                               {-rotRateEst[1], rotRateEst[0], 0}};
 
-  // ToDo
   // Discrete Process Noise Covariance Q
   double discProcessNoiseCov[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}};
@@ -1072,7 +1046,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   discProcessNoiseCov[5][4] = covQ22[2][1];
   discProcessNoiseCov[5][5] = covQ22[2][2];
 
-  // ToDo
   // State Transition Matrix phi
   double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
          phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},