first version for gauss-jordan matrix inversion

mission/controller/acs/util/MathOperations.h

@@ -1,19 +1,14 @@
-/*
- * MathOperations.h
- *
- *  Created on: 3 Mar 2022
- *      Author: rooob
- */
-
 #ifndef MATH_MATHOPERATIONS_H_
 #define MATH_MATHOPERATIONS_H_
 
-#include <math.h>
-#include <sys/time.h>
-#include <stdint.h>
-#include <string.h>
 #include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <math.h>
+#include <stdint.h>
+#include <string.h>
+#include <sys/time.h>
+
+#include <iostream>
 
 using namespace Math;
 
@@ -32,14 +27,13 @@ public:
     result[7] = vector[0];
     result[8] = 0;
   }
-	static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[],
+  static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
-			T2 *result, uint8_t size = 3) {
+                                    uint8_t size = 3) {
     // Looks like MatrixOpertions::multiply is able to do the same thing
     for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
       for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
-				result[resultColumn + size * resultRow] = vector1[resultRow]
+        result[resultColumn + size * resultRow] =
-						* transposeVector2[resultColumn];
+            vector1[resultRow] * transposeVector2[resultColumn];
-
       }
     }
     /*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
@@ -48,8 +42,7 @@ public:
      matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
   }
 
-	static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize,
+  static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
-			uint8_t colSize) {
     int min_idx;
     T1 temp;
     memcpy(result, matrix, rowSize * colSize * sizeof(*result));
@@ -59,8 +52,7 @@ public:
         // Find the minimum element in unsorted array
         min_idx = i;
         for (int j = i + 1; j < colSize; j++) {
-					if (result[j + k * colSize]
+          if (result[j + k * colSize] < result[min_idx + k * colSize]) {
-							< result[min_idx + k * colSize]) {
             min_idx = j;
           }
         }
@@ -73,11 +65,11 @@ public:
   }
 
   static void convertDateToJD2000(const T1 time, T2 julianDate) {
-
     // time = { Y, M, D, h, m,s}
     // time in sec and microsec -> The Epoch (unixtime)
-				julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12))
+    julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
-						+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440;
+                 floor(275 * time[1] / 9) + time[2] +
+                 (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
   }
 
   static T1 convertUnixToJD2000(timeval time) {
@@ -100,11 +92,10 @@ public:
     outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
     outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
     outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
-
   }
 
-	static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){
+  static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
-
+                                      T2 *cartesianOutput) {
     double radiusPolar = 6378137;
     double radiusEqua = 6356752.314;
 
@@ -114,7 +105,6 @@ public:
     cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
     cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
     cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
-
   }
 
   /* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
@@ -123,15 +113,13 @@ public:
    * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
    * 			Landis Markley and John L. Crassidis*/
   static void dcmEJ(timeval time, T1 *outputDcmEJ) {
-
     double JD2000Floor = 0;
     double JD2000 = convertUnixToJD2000(time);
     // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
     JD2000Floor = floor(JD2000);
     if ((JD2000 - JD2000Floor) < 0.5) {
       JD2000Floor -= 0.5;
-		}
+    } else {
-		else {
       JD2000Floor += 0.5;
     }
 
@@ -155,7 +143,6 @@ public:
     outputDcmEJ[6] = 0;
     outputDcmEJ[7] = 0;
     outputDcmEJ[8] = 1;
-
   }
 
   /* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
@@ -166,11 +153,11 @@ public:
    * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
                           Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
    * 			Oliver Zeile
-	 * 			https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
+   *
+   https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
   static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
-
+    //		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
-//		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset)
+    //(initial Offset) 		International Atomic Time (TAI)
-//		International Atomic Time (TAI)
 
     double JD2000UTC1 = convertUnixToJD2000(unixTime);
 
@@ -228,7 +215,7 @@ public:
     precession[2][2] = cos(theta2);
 
     //-------------------------------------------------------------------------------------
-//		Calculation of Transformation from earth Nutation N
+    //		Calculation of Transformation from earth Nutation size
     //-------------------------------------------------------------------------------------
     double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
     //      lunar asc node
@@ -242,7 +229,8 @@ public:
     double de = 9.203 * arcsecFactor * cos(Om);
 
     //        % true obliquity of the ecliptic eps p.71 (simplified)
-        double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;;
+    double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
+    ;
 
     nutation[0][0] = cos(dp);
     nutation[1][0] = cos(e + de) * sin(dp);
@@ -271,28 +259,109 @@ public:
     MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
 
     MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
-
   }
 
   static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
-
     int i, j;
     double determinant;
-		double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]},
+    double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
+                        {matrix[3], matrix[4], matrix[5]},
                         {matrix[6], matrix[7], matrix[8]}};
 
     for (i = 0; i < 3; i++) {
-		determinant = determinant + (mat[0][i] * (mat[1][(i+1)%3] * mat[2][(i+2)%3] - mat[1][(i+2)%3] * mat[2][(i+1)%3]));
+      determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
+                                                mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
     }
-//		cout<<"\n\ndeterminant: "<<determinant;
+    //		cout<<"\size\ndeterminant: "<<determinant;
-//		cout<<"\n\nInverse of matrix is: \n";
+    //		cout<<"\size\nInverse of matrix is: \size";
     for (i = 0; i < 3; i++) {
       for (j = 0; j < 3; j++) {
-			output[i*3+j] = ((mat[(j+1)%3][(i+1)%3] * mat[(j+2)%3][(i+2)%3]) - (mat[(j+1)%3][(i+2)%3] * mat[(j+2)%3][(i+1)%3]))/ determinant;
+        output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
+                             (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
+                            determinant;
       }
     }
   }
 
+  static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
+    float det = 0;
+    T1 matrix[size][size], submatrix[size][size];
+    for (uint8_t row = 0; row < size; row++) {
+      for (uint8_t col = 0; col < size; col++) {
+        matrix[row][col] = inputMatrix[row * size + col];
+      }
+    }
+    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++) {
+        int subi = 0;
+        for (uint8_t i = 1; i < size; i++) {
+          int subj = 0;
+          for (uint8_t j = 0; j < size; j++) {
+            if (j == x) continue;
+            submatrix[subi][subj] = matrix[i][j];
+            subj++;
+          }
+          subi++;
+        }
+        det = det + (pow(-1, x) * matrix[0][x] *
+                     MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
+      }
+    }
+    return det;
+  }
+
+  static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
+    if (MathOperations<T1>::matrixDeterminant(*inputMatrix, size) == 0) {
+      return 0;  // Matrix is singular and not invertible
+    }
+    T1 matrix[size][size], identity[size][size] = {0};
+    // reformat array to matrix
+    for (uint8_t row = 0; row < size; row++) {
+      for (uint8_t col = 0; col < size; col++) {
+        matrix[row][col] = inputMatrix[row * size + col];
+      }
+    }
+    // init identity matrix
+    for (uint8_t diag = 0; diag < size; diag++) {
+      identity[diag][diag] = 1;
+    }
+    // gauss-jordan algo
+    for (uint8_t row = 0; row < size; row++) {
+      uint8_t rowIndex = row;
+      // check if diag entry is 0
+      // in case it is, find next row whose diag entry is not 0
+      while (matrix[rowIndex][row] == 0) {
+        if (rowIndex < size) {
+          rowIndex++;
+        } else {
+          return 0;  // 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]);
+        }
+      }
+      // normalize line
+      for (uint8_t colIndex = row; colIndex < size; colIndex++) {
+        matrix[row][colIndex] = matrix[row][colIndex] / matrix[row][row];
+        identity[row][colIndex] = identity[row][colIndex] / matrix[row][row];
+      }
+      // make elements of the same col in following rows to 0
+      for (uint8_t rowIndex = row + 1; rowIndex < size; rowIndex++) {
+        for (uint8_t colIndex = row; colIndex < size; colIndex++) {
+          matrix[rowIndex][colIndex] -= matrix[row][colIndex] * matrix[rowIndex][row];
+          identity[rowIndex][colIndex] -= identity[row][colIndex] * matrix[rowIndex][row];
+        }
+      }
+    }
+    std::memcpy(inverse, identity, size * size * sizeof(T1));
+    return 1;  // successful inversion
+  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */