diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index c240b9ed..7bf7319b 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -6,6 +6,10 @@
 
 #include <cmath>
 
+// Most of these functions already exist within the FSFW and are redundant
+// All others should have been merged into the FSFW
+// So if you are that bored, that you are reading this, you were better of merging these now
+
 template <typename T1, typename T2 = T1>
 class MathOperations {
  public:
@@ -361,7 +365,7 @@ class MathOperations {
     return det;
   }
 
-  static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
+  static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
     // Stopwatch stopwatch;
     T1 matrix[size][size], identity[size][size];
     // reformat array to matrix
@@ -392,7 +396,7 @@ class MathOperations {
           rowIndex++;
         }
         if (!swaped) {
-          return 1;  // matrix not invertible
+          return returnvalue::FAILED;  // matrix not invertible
         }
       }
     }
@@ -411,7 +415,7 @@ class MathOperations {
         }
         row--;
         if (row < 0) {
-          return 1;  // Matrix is not invertible
+          return returnvalue::FAILED;  // Matrix is not invertible
         }
       }
     }
@@ -434,12 +438,12 @@ class MathOperations {
       }
     }
     std::memcpy(inverse, identity, sizeof(identity));
-    return 0;  // successful inversion
+    return returnvalue::OK;  // successful inversion
   }
 
   static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
     for (uint8_t i = 0; i < size; i++) {
-      if (not isfinite(inputVector[i])) {
+      if (not std::isfinite(inputVector[i])) {
         return false;
       }
     }
@@ -449,13 +453,26 @@ class MathOperations {
   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])) {
+        if (not std::isfinite(inputMatrix[row * cols + col])) {
           return false;
         }
       }
     }
     return true;
   }
+
+  static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols,
+                             uint8_t mainRows, uint8_t mainCols, uint8_t startRow,
+                             uint8_t startCol) {
+    if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) {
+      return;
+    }
+    for (uint8_t row = 0; row < subRows; row++) {
+      for (uint8_t col = 0; col < subCols; col++) {
+        mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col];
+      }
+    }
+  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */