From 95a4d88ed465489ca9b9a26e5570134d2c50e19f Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Mar 2023 10:17:24 +0100
Subject: [PATCH] cleanup

---
 .../acs/MultiplicativeKalmanFilter.cpp        |  7 -------
 .../acs/MultiplicativeKalmanFilter.h          |  6 ++----
 mission/controller/acs/util/MathOperations.h  | 20 -------------------
 3 files changed, 2 insertions(+), 31 deletions(-)

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index e752da39..77a3ef00 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1080,12 +1080,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
-  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
-      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
-    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
-    return MEKF_NOT_FINITE;
-  }
-
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
   return MEKF_RUNNING;
 }
@@ -1095,7 +1089,6 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   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(propagatedQuaternion, resetQuaternion, 4 * sizeof(double));
   std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
   updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
   return MEKF_UNINITIALIZED;
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index fe749890..ceb98339 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -62,7 +62,6 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -75,9 +74,8 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
   static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
       returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
 
  private:
   /*Parameters*/
diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index b344451a..f8537740 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -404,26 +404,6 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
-
-  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
-    for (uint8_t i = 0; i < size; i++) {
-      if (not isfinite(inputVector[i])) {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
-    for (uint8_t col = 0; col < cols; col++) {
-      for (uint8_t row = 0; row < rows; row++) {
-        if (not isfinite(inputMatrix[row * cols + cols])) {
-          return false;
-        }
-      }
-    }
-    return true;
-  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */