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_ */