From b2e0ef24f3825bdd03e1739c93d3fd1444423379 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 29 Nov 2022 11:45:58 +0100 Subject: [PATCH] first version for gauss-jordan matrix inversion --- mission/controller/acs/util/MathOperations.h | 577 +++++++++++-------- 1 file changed, 323 insertions(+), 254 deletions(-) diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index 6a534880..c37dbde8 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -1,298 +1,367 @@ -/* - * MathOperations.h - * - * Created on: 3 Mar 2022 - * Author: rooob - */ - #ifndef MATH_MATHOPERATIONS_H_ #define MATH_MATHOPERATIONS_H_ -#include -#include -#include -#include #include #include +#include +#include +#include +#include + +#include using namespace Math; -template +template class MathOperations { -public: - static void skewMatrix(const T1 vector[], T2 *result) { - // Input Dimension [3], Output [3][3] - result[0] = 0; - result[1] = -vector[2]; - result[2] = vector[1]; - result[3] = vector[2]; - result[4] = 0; - result[5] = -vector[0]; - result[6] = -vector[1]; - result[7] = vector[0]; - result[8] = 0; - } - static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], - T2 *result, 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] - * transposeVector2[resultColumn]; + public: + static void skewMatrix(const T1 vector[], T2 *result) { + // Input Dimension [3], Output [3][3] + result[0] = 0; + result[1] = -vector[2]; + result[2] = vector[1]; + result[3] = vector[2]; + result[4] = 0; + result[5] = -vector[0]; + result[6] = -vector[1]; + result[7] = vector[0]; + result[8] = 0; + } + static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result, + 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] * transposeVector2[resultColumn]; + } + } + /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; + matrixMag[i][j] = magEstB[i] * magEstB[j]; + matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; + matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ + } - } - } - /*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; - matrixMag[i][j] = magEstB[i] * magEstB[j]; - matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; - matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ - } + static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) { + int min_idx; + T1 temp; + memcpy(result, matrix, rowSize * colSize * sizeof(*result)); + // One by one move boundary of unsorted subarray + for (int k = 0; k < rowSize; k++) { + for (int i = 0; i < colSize - 1; i++) { + // Find the minimum element in unsorted array + min_idx = i; + for (int j = i + 1; j < colSize; j++) { + if (result[j + k * colSize] < result[min_idx + k * colSize]) { + min_idx = j; + } + } + // Swap the found minimum element with the first element + temp = result[i + k * colSize]; + result[i + k * colSize] = result[min_idx + k * colSize]; + result[min_idx + k * colSize] = temp; + } + } + } - static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, - uint8_t colSize) { - int min_idx; - T1 temp; - memcpy(result, matrix, rowSize * colSize * sizeof(*result)); - // One by one move boundary of unsorted subarray - for (int k = 0; k < rowSize; k++) { - for (int i = 0; i < colSize - 1; i++) { - // Find the minimum element in unsorted array - min_idx = i; - for (int j = i + 1; j < colSize; j++) { - if (result[j + k * colSize] - < result[min_idx + k * colSize]) { - min_idx = j; - } - } - // Swap the found minimum element with the first element - temp = result[i + k * colSize]; - result[i + k * colSize] = result[min_idx + k * colSize]; - result[min_idx + k * colSize] = temp; - } - } - } + 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)) + + floor(275 * time[1] / 9) + time[2] + + (60 * time[3] + time[4] + (time(5) / 60)) / 1440; + } - static void convertDateToJD2000(const T1 time, T2 julianDate){ + static T1 convertUnixToJD2000(timeval time) { + // time = {{s},{us}} + T1 julianDate2000; + julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545; + return julianDate2000; + } - // 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)) - +floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440; - } + static void dcmFromQuat(const T1 vector[], T1 *outputDcm) { + // convention q = [qx,qy,qz, qw] + outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]); + outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]); - static T1 convertUnixToJD2000(timeval time){ - //time = {{s},{us}} - T1 julianDate2000; - julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545; - return julianDate2000; - } + outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]); + outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2); + outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]); - static void dcmFromQuat(const T1 vector[], T1 *outputDcm){ - // convention q = [qx,qy,qz, qw] - outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); - outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]); - outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]); + 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); + } - outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]); - outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); - outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]); + static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, + T2 *cartesianOutput) { + double radiusPolar = 6378137; + double radiusEqua = 6356752.314; - 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); + double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2)); + double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2)); - } + 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); + } - static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){ + /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame + * @param: time Current time + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * @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 { + JD2000Floor += 0.5; + } - double radiusPolar = 6378137; - double radiusEqua = 6356752.314; + double JC2000 = JD2000Floor / 36525; + double sec = (JD2000 - JD2000Floor) * 86400; + double gmst = 0; // greenwich mean sidereal time + gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) - + 0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec; + double rest = gmst / 86400; + double FloorRest = floor(rest); + double secOfDay = rest - FloorRest; + secOfDay *= 86400; + gmst = secOfDay / 240 * PI / 180; - double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2)); - double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); + outputDcmEJ[0] = cos(gmst); + outputDcmEJ[1] = sin(gmst); + outputDcmEJ[2] = 0; + outputDcmEJ[3] = -sin(gmst); + outputDcmEJ[4] = cos(gmst); + outputDcmEJ[5] = 0; + outputDcmEJ[6] = 0; + outputDcmEJ[7] = 0; + outputDcmEJ[8] = 1; + } - 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: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame + * give also the back the derivative of this matrix + * @param: unixTime Current time in Unix format + * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] + * outputDotDcmEJ Derivative of transformation matrix [3][3] + * @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*/ + static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { + // TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 + //(initial Offset) International Atomic Time (TAI) - } + double JD2000UTC1 = convertUnixToJD2000(unixTime); - /* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame - * @param: time Current time - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - static void dcmEJ(timeval time, T1 * outputDcmEJ){ + // Julian Date / century from TT + timeval terestrialTime = unixTime; + terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; + double JD2000TT = convertUnixToJD2000(terestrialTime); + double JC2000TT = JD2000TT / 36525; - 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 { - JD2000Floor += 0.5; - } + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth rotation Theta + //------------------------------------------------------------------------------------- + double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // Earth Rotation angle + double era = 0; + era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1); + // Greenwich Mean Sidereal Time + double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) - + 0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4); + double arcsecFactor = 1 * PI / (180 * 3600); + gmst2000 *= arcsecFactor; + gmst2000 += era; - double JC2000 = JD2000Floor / 36525; - double sec = (JD2000 - JD2000Floor) * 86400; - double gmst = 0; //greenwich mean sidereal time - gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) - - 0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec; - double rest = gmst / 86400; - double FloorRest = floor(rest); - double secOfDay = rest-FloorRest; - secOfDay *= 86400; - gmst = secOfDay / 240 * PI / 180; + theta[0][0] = cos(gmst2000); + theta[0][1] = sin(gmst2000); + theta[0][2] = 0; + theta[1][0] = -sin(gmst2000); + theta[1][1] = cos(gmst2000); + theta[1][2] = 0; + theta[2][0] = 0; + theta[2][1] = 0; + theta[2][2] = 1; - outputDcmEJ[0] = cos(gmst); - outputDcmEJ[1] = sin(gmst); - outputDcmEJ[2] = 0; - outputDcmEJ[3] = -sin(gmst); - outputDcmEJ[4] = cos(gmst); - outputDcmEJ[5] = 0; - outputDcmEJ[6] = 0; - outputDcmEJ[7] = 0; - outputDcmEJ[8] = 1; + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Precession P + //------------------------------------------------------------------------------------- + double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - } + double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3); + double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3); + double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3); - /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame - * give also the back the derivative of this matrix - * @param: unixTime Current time in Unix format - * outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @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*/ - static void ecfToEciWithNutPre(timeval unixTime, T1 * outputDcmEJ, T1 * outputDotDcmEJ ) { + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; -// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset) -// International Atomic Time (TAI) + precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta); + precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta); + precession[2][0] = sin(theta2) * cos(zeta); + precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta); + precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta); + precession[2][1] = -sin(theta2) * sin(zeta); + precession[0][2] = -cos(ze) * sin(theta2); + precession[1][2] = -sin(ze) * sin(theta2); + precession[2][2] = cos(theta2); - double JD2000UTC1 = convertUnixToJD2000(unixTime); + //------------------------------------------------------------------------------------- + // Calculation of Transformation from earth Nutation size + //------------------------------------------------------------------------------------- + double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + // lunar asc node + double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + + 7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3); + Om *= arcsecFactor; + // delta psi approx + double dp = -17.2 * arcsecFactor * sin(Om); -// Julian Date / century from TT - timeval terestrialTime = unixTime; - terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37; - double JD2000TT = convertUnixToJD2000(terestrialTime); - double JC2000TT = JD2000TT / 36525; + // delta eps approx + double de = 9.203 * arcsecFactor * cos(Om); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth rotation Theta -//------------------------------------------------------------------------------------- - double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// Earth Rotation angle - double era = 0; - era = 2* PI *(0.779057273264 + 1.00273781191135448 * JD2000UTC1); -// Greenwich Mean Sidereal Time - double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT,2) - - 0.00009344 * pow(JC2000TT,3) + 0.00001882 * pow(JC2000TT,4); - double arcsecFactor = 1 * PI / (180 * 3600); - gmst2000 *= arcsecFactor; - gmst2000 += era; + // % true obliquity of the ecliptic eps p.71 (simplified) + double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180; + ; - theta[0][0] = cos(gmst2000); - theta[0][1] = sin(gmst2000); - theta[0][2] = 0; - theta[1][0] = -sin(gmst2000); - theta[1][1] = cos(gmst2000); - theta[1][2] = 0; - theta[2][0] = 0; - theta[2][1] = 0; - theta[2][2] = 1; + nutation[0][0] = cos(dp); + nutation[1][0] = cos(e + de) * sin(dp); + nutation[2][0] = sin(e + de) * sin(dp); + nutation[0][1] = -cos(e) * sin(dp); + nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de); + nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de); + nutation[0][2] = -sin(e) * sin(dp); + nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de); + nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de); -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Precession P -//------------------------------------------------------------------------------------- - double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; + //------------------------------------------------------------------------------------- + // Calculation of Derivative of rotation matrix from earth + //------------------------------------------------------------------------------------- + double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; + double omegaEarth = 0.000072921158553; + MatrixOperations::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); + MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT,2) + 0.017998 * pow(JC2000TT,3); - double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3); - double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3); + //------------------------------------------------------------------------------------- + // Calculation of transformation matrix and Derivative of transformation matrix + //------------------------------------------------------------------------------------- + double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); + MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); - zeta *= arcsecFactor; - theta2 *= arcsecFactor; - ze *= arcsecFactor; + MatrixOperations::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3); + } - precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta); - precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta); - precession[2][0]=sin(theta2)*cos(zeta); - precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta); - precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta); - precession[2][1]=-sin(theta2)*sin(zeta); - precession[0][2]=-cos(ze)*sin(theta2); - precession[1][2]=-sin(ze)*sin(theta2); - precession[2][2]=cos(theta2); + 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]}, + {matrix[6], matrix[7], matrix[8]}}; -//------------------------------------------------------------------------------------- -// Calculation of Transformation from earth Nutation N -//------------------------------------------------------------------------------------- - double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; -// lunar asc node - double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + - 7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3); - Om *= arcsecFactor; -// delta psi approx - double dp = -17.2 * arcsecFactor *sin(Om); + 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])); + } + // cout<<"\size\ndeterminant: "<::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); - MatrixOperations::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); - -//------------------------------------------------------------------------------------- -// Calculation of transformation matrix and Derivative of transformation matrix -//------------------------------------------------------------------------------------- - double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; - MatrixOperations::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3); - MatrixOperations::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3); - - MatrixOperations::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]}, - {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])); - } -// cout<<"\n\ndeterminant: "<::matrixDeterminant(*submatrix, size - 1)); + } + } + return det; + } + static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { + if (MathOperations::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_ */