working versions of inverse calc and determinant calc

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -888,14 +888,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   // (H * P * H' + R)
   MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
   // <<INVERSE residualCov HIER>>
-  // double invResidualCov1[MDF] = {0};
   double invResidualCov[MDF][MDF] = {{0}};
-  //  int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov,
-  //  *invResidualCov,
-  //                                                                      invResidualCov1, MDF);
   int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
-  double test[MDF][MDF];
-  MatrixOperations<double>::multiply(*residualCov, *invResidualCov, *test, MDF, MDF, MDF);
   if (inversionFailed) {
     {
       PoolReadGuard pg(mekfData);

mission/controller/acs/util/MathOperations.h

@@ -285,7 +285,7 @@ class MathOperations {
 
   static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
     float det = 0;
-    T1 matrix[size][size], submatrix[size][size];
+    T1 matrix[size][size], submatrix[size - 1][size - 1];
     for (uint8_t row = 0; row < size; row++) {
       for (uint8_t col = 0; col < size; col++) {
         matrix[row][col] = inputMatrix[row * size + col];
@@ -294,18 +294,18 @@ class MathOperations {
     if (size == 2)
       return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
     else {
-      for (uint8_t x = 0; x < size; x++) {
+      for (uint8_t col = 0; col < size; col++) {
-        int subi = 0;
+        int subRow = 0;
-        for (uint8_t i = 1; i < size; i++) {
+        for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
-          int subj = 0;
+          int subCol = 0;
-          for (uint8_t j = 0; j < size; j++) {
+          for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
-            if (j == x) continue;
+            if (colIndex == col) continue;
-            submatrix[subi][subj] = matrix[i][j];
+            submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
-            subj++;
+            subCol++;
           }
-          subi++;
+          subRow++;
         }
-        det = det + (pow(-1, x) * matrix[0][x] *
+        det += (pow(-1, col) * matrix[0][col] *
                 MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
       }
     }
@@ -313,7 +313,6 @@ class MathOperations {
   }
 
   static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
-    std::cout << MathOperations<T1>::matrixDeterminant(inputMatrix, size) << std::endl;
     if (MathOperations<T1>::matrixDeterminant(inputMatrix, size) == 0) {
       return 1;  // Matrix is singular and not invertible
     }
@@ -330,66 +329,64 @@ class MathOperations {
       identity[diag][diag] = 1;
     }
     // gauss-jordan algo
-    // start with gauss
+    // sort matrix such as no diag entry shall be 0
+    // should not be needed as such a matrix has a det=0
     for (uint8_t row = 0; row < size; row++) {
-      uint8_t rowIndex = row;
+      if (matrix[row][row] == 0.0) {
-      // check if diag entry is 0
+        bool swaped = false;
-      // in case it is, find next row whose diag entry is not 0
+        uint8_t rowIndex = 0;
-      while (matrix[rowIndex][row] == 0) {
+        while ((rowIndex < size) && !swaped) {
-        if (rowIndex < size) {
+          if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
-          rowIndex++;
-        } else {
-          return 1;  // Matrix is not invertible
-        }
-      }
-      // swap rows if needed
-      if (rowIndex != row) {
             for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
               std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
               std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
             }
+            swaped = true;
           }
-      // normalize line
+          rowIndex++;
-      double normFactor = matrix[row][row];
+        }
-      for (uint8_t colIndex = row; colIndex < size; colIndex++) {
+        if (!swaped) {
-        matrix[row][colIndex] /= normFactor;
+          return 1;  // matrix not invertible
-        identity[row][colIndex] /= normFactor;
+        }
+      }
+    }
+
+    for (int row = 0; row < size; row++) {
+      if (matrix[row][row] == 0.0) {
+        uint8_t rowIndex;
+        if (row == 0) {
+          rowIndex = size - 1;
+        } else {
+          rowIndex = row - 1;
         }
-      // make elements of the same col in following rows to 0
-      std::cout << "C++ sucks" << std::endl;
-      for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) {
-        double elimFactor = matrix[rowIndex][row];
         for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
-          matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor;
+          std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
-          identity[rowIndex][colIndex] -= identity[row][colIndex] * elimFactor;
+          std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
+        }
+        row--;
+        if (row < 0) {
+          return 1;  // Matrix is not invertible
         }
       }
     }
-    // finish with jordan
+    // remove non diag elements in matrix (jordan)
-    for (uint8_t row = size - 1; row > 0; row--) {
+    for (int row = 0; row < size; row++) {
-      for (int16_t rowIndex = row - 1; rowIndex >= 0; rowIndex--) {
+      for (int rowIndex = 0; rowIndex < size; rowIndex++) {
-        double elimFactor = matrix[rowIndex][row];
+        if (row != rowIndex) {
-        for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
+          double ratio = matrix[rowIndex][row] / matrix[row][row];
-          matrix[rowIndex][colIndex] -= matrix[row][colIndex] * elimFactor;
+          for (int colIndex = 0; colIndex < size; colIndex++) {
-          identity[rowIndex][row] -= identity[row][colIndex] * elimFactor;
+            matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
+            identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
           }
         }
       }
-    T1 test[size][size];
+    }
-    MatrixOperations<T1>::multiply(inputMatrix, *identity, *test, size, size, size);
+    // normalize rows in matrix (gauss)
-    std::cout << "[\n"
+    for (int row = 0; row < size; row++) {
-              << test[0][0] << " " << test[0][1] << " " << test[0][2] << " " << test[0][3] << " "
+      for (int col = 0; col < size; col++) {
-              << test[0][4] << " " << test[0][5] << "\n"
+        identity[row][col] = identity[row][col] / matrix[row][row];
-              << test[1][0] << " " << test[1][1] << " " << test[1][2] << " " << test[1][3] << " "
+      }
-              << test[1][4] << " " << test[1][5] << "\n"
+    }
-              << test[2][0] << " " << test[2][1] << " " << test[2][2] << " " << test[2][3] << " "
-              << test[2][4] << " " << test[2][5] << "\n"
-              << test[3][0] << " " << test[3][1] << " " << test[3][2] << " " << test[3][3] << " "
-              << test[3][4] << " " << test[3][5] << "\n"
-              << test[4][0] << " " << test[4][1] << " " << test[4][2] << " " << test[4][3] << " "
-              << test[4][4] << " " << test[4][5] << "\n"
-              << test[5][0] << " " << test[5][1] << " " << test[5][2] << " " << test[5][3] << " "
-              << test[5][4] << " " << test[5][5] << "\n]" << std::endl;
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }