?

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -1080,6 +1080,12 @@ 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;
 }

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -74,8 +75,9 @@ 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_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
+  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);
 
  private:
   /*Parameters*/

mission/controller/acs/util/MathOperations.h

@@ -404,6 +404,26 @@ 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_ */