eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

first version for gauss-jordan matrix inversion

Browse Source
This commit is contained in:
Marius Eggert 2022-11-29 11:45:58 +01:00
parent 2d939a2894
commit b2e0ef24f3
1 changed files with 323 additions and 254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

577
mission/controller/acs/util/MathOperations.h
View File

@ -1,298 +1,367 @@
/*
* MathOperations.h
*
* Created on: 3 Mar 2022
* Author: rooob
*/
#ifndef MATH_MATHOPERATIONS_H_ #ifndef MATH_MATHOPERATIONS_H_
#define 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/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.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; using namespace Math;
template<typename T1, typename T2 = T1> template <typename T1, typename T2 = T1>
class MathOperations { class MathOperations {
public: public:
static void skewMatrix(const T1 vector[], T2 *result) { static void skewMatrix(const T1 vector[], T2 *result) {
// Input Dimension [3], Output [3][3] // Input Dimension [3], Output [3][3]
result[0] = 0; result[0] = 0;
result[1] = -vector[2]; result[1] = -vector[2];
result[2] = vector[1]; result[2] = vector[1];
result[3] = vector[2]; result[3] = vector[2];
result[4] = 0; result[4] = 0;
result[5] = -vector[0]; result[5] = -vector[0];
result[6] = -vector[1]; result[6] = -vector[1];
result[7] = vector[0]; result[7] = vector[0];
result[8] = 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 // Looks like MatrixOpertions::multiply is able to do the same thing
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) { for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
for (uint8_t resultRow = 0; resultRow < size; resultRow++) { 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];
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;
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j]; T1 temp;
matrixMag[i][j] = magEstB[i] * magEstB[j]; memcpy(result, matrix, rowSize * colSize * sizeof(*result));
matrixSunMag[i][j] = sunEstB[i] * magEstB[j]; // One by one move boundary of unsorted subarray
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/ 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, static void convertDateToJD2000(const T1 time, T2 julianDate) {
uint8_t colSize) { // time = { Y, M, D, h, m,s}
int min_idx; // time in sec and microsec -> The Epoch (unixtime)
T1 temp; julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
memcpy(result, matrix, rowSize * colSize * sizeof(*result)); floor(275 * time[1] / 9) + time[2] +
// One by one move boundary of unsorted subarray (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
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){ 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} static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
// time in sec and microsec -> The Epoch (unixtime) // convention q = [qx,qy,qz, qw]
julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12)) outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440; 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){ outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
//time = {{s},{us}} outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
T1 julianDate2000; outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545;
return julianDate2000;
}
static void dcmFromQuat(const T1 vector[], T1 *outputDcm){ outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
// convention q = [qx,qy,qz, qw] outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); outputDcm[8] = -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[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]); static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2); T2 *cartesianOutput) {
outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]); double radiusPolar = 6378137;
double radiusEqua = 6356752.314;
outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]); double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]); double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
outputDcm[8] = -pow(vector[0],2) - pow(vector[1],2) + pow(vector[2],2) + pow(vector[3],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 JC2000 = JD2000Floor / 36525;
double radiusEqua = 6356752.314; 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)); outputDcmEJ[0] = cos(gmst);
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2)); 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); /* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi); * give also the back the derivative of this matrix
cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat); * @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 // Julian Date / century from TT
* @param: time Current time timeval terestrialTime = unixTime;
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3] terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff double JD2000TT = convertUnixToJD2000(terestrialTime);
* Landis Markley and John L. Crassidis*/ double JC2000TT = JD2000TT / 36525;
static void dcmEJ(timeval time, T1 * outputDcmEJ){
double JD2000Floor = 0; //-------------------------------------------------------------------------------------
double JD2000 = convertUnixToJD2000(time); // Calculation of Transformation from earth rotation Theta
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0) //-------------------------------------------------------------------------------------
JD2000Floor = floor(JD2000); double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
if ( ( JD2000 - JD2000Floor) < 0.5) { // Earth Rotation angle
JD2000Floor -= 0.5; double era = 0;
} era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
else { // Greenwich Mean Sidereal Time
JD2000Floor += 0.5; 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; theta[0][0] = cos(gmst2000);
double sec = (JD2000 - JD2000Floor) * 86400; theta[0][1] = sin(gmst2000);
double gmst = 0; //greenwich mean sidereal time theta[0][2] = 0;
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) - theta[1][0] = -sin(gmst2000);
0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec; theta[1][1] = cos(gmst2000);
double rest = gmst / 86400; theta[1][2] = 0;
double FloorRest = floor(rest); theta[2][0] = 0;
double secOfDay = rest-FloorRest; theta[2][1] = 0;
secOfDay *= 86400; theta[2][2] = 1;
gmst = secOfDay / 240 * PI / 180;
outputDcmEJ[0] = cos(gmst); //-------------------------------------------------------------------------------------
outputDcmEJ[1] = sin(gmst); // Calculation of Transformation from earth Precession P
outputDcmEJ[2] = 0; //-------------------------------------------------------------------------------------
outputDcmEJ[3] = -sin(gmst); double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
outputDcmEJ[4] = cos(gmst);
outputDcmEJ[5] = 0;
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
} 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 zeta *= arcsecFactor;
* give also the back the derivative of this matrix theta2 *= arcsecFactor;
* @param: unixTime Current time in Unix format ze *= arcsecFactor;
* 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) precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
// International Atomic Time (TAI) 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 // delta eps approx
timeval terestrialTime = unixTime; double de = 9.203 * arcsecFactor * cos(Om);
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
double JD2000TT = convertUnixToJD2000(terestrialTime);
double JC2000TT = JD2000TT / 36525;
//------------------------------------------------------------------------------------- // % true obliquity of the ecliptic eps p.71 (simplified)
// Calculation of Transformation from earth rotation Theta double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
//------------------------------------------------------------------------------------- ;
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;
theta[0][0] = cos(gmst2000); nutation[0][0] = cos(dp);
theta[0][1] = sin(gmst2000); nutation[1][0] = cos(e + de) * sin(dp);
theta[0][2] = 0; nutation[2][0] = sin(e + de) * sin(dp);
theta[1][0] = -sin(gmst2000); nutation[0][1] = -cos(e) * sin(dp);
theta[1][1] = cos(gmst2000); nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
theta[1][2] = 0; nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
theta[2][0] = 0; nutation[0][2] = -sin(e) * sin(dp);
theta[2][1] = 0; nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
theta[2][2] = 1; nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Precession P // Calculation of Derivative of rotation matrix from earth
//------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------
double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; 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<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
MatrixOperations<double>::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); // Calculation of transformation matrix and Derivative of transformation matrix
double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3); //-------------------------------------------------------------------------------------
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
zeta *= arcsecFactor; MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
theta2 *= arcsecFactor; }
ze *= arcsecFactor;
precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta); static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta); int i, j;
precession[2][0]=sin(theta2)*cos(zeta); double determinant;
precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta); double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta); {matrix[3], matrix[4], matrix[5]},
precession[2][1]=-sin(theta2)*sin(zeta); {matrix[6], matrix[7], matrix[8]}};
precession[0][2]=-cos(ze)*sin(theta2);
precession[1][2]=-sin(ze)*sin(theta2);
precession[2][2]=cos(theta2);
//------------------------------------------------------------------------------------- for (i = 0; i < 3; i++) {
// Calculation of Transformation from earth Nutation N 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]));
double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; }
// lunar asc node // cout<<"\size\ndeterminant: "<<determinant;
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT + // cout<<"\size\nInverse of matrix is: \size";
7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3); for (i = 0; i < 3; i++) {
Om *= arcsecFactor; for (j = 0; j < 3; j++) {
// delta psi approx output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
double dp = -17.2 * arcsecFactor *sin(Om); (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
determinant;
}
}
}
// delta eps approx static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
double de = 9.203 * arcsecFactor *cos(Om); float det = 0;
T1 matrix[size][size], submatrix[size][size];
// % true obliquity of the ecliptic eps p.71 (simplified) for (uint8_t row = 0; row < size; row++) {
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;; for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
nutation[0][0]=cos(dp); }
nutation[1][0]=cos(e+de)*sin(dp); }
nutation[2][0]=sin(e+de)*sin(dp); if (size == 2)
nutation[0][1]=-cos(e)*sin(dp); return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de); else {
nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de); for (uint8_t x = 0; x < size; x++) {
nutation[0][2]=-sin(e)*sin(dp); int subi = 0;
nutation[1][2]=sin(e)*cos(e+de)*cos(dp)-cos(e)*sin(e+de); for (uint8_t i = 1; i < size; i++) {
nutation[2][2]=sin(e)*sin(e+de)*cos(dp)+cos(e)*cos(e+de); int subj = 0;
for (uint8_t j = 0; j < size; j++) {
//------------------------------------------------------------------------------------- if (j == x) continue;
// Calculation of Derivative of rotation matrix from earth submatrix[subi][subj] = matrix[i][j];
//------------------------------------------------------------------------------------- subj++;
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}}; subi++;
double omegaEarth = 0.000072921158553; }
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3); det = det + (pow(-1, x) * matrix[0][x] *
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3); MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
}
//------------------------------------------------------------------------------------- }
// Calculation of transformation matrix and Derivative of transformation matrix return det;
//------------------------------------------------------------------------------------- }
double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
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]},
{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: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
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;
}
}
}
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_ */ #endif /* ACS_MATH_MATHOPERATIONS_H_ */