diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index de4659d8..fbdb481e 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include using namespace Math; diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h new file mode 100644 index 00000000..57d9b7b4 --- /dev/null +++ b/mission/controller/acs/util/MathOperations.h @@ -0,0 +1,297 @@ +/* + * MathOperations.h + * + * Created on: 3 Mar 2022 + * Author: rooob + */ + +#ifndef MATH_MATHOPERATIONS_H_ +#define MATH_MATHOPERATIONS_H_ + +#include +#include +#include +#include +#include + +using namespace Math; + +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]; + + } + } + /*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 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 T1 convertUnixToJD2000(timeval time){ + //time = {{s},{us}} + T1 julianDate2000; + julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545; + return julianDate2000; + } + + 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[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]); + + 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){ + + double radiusPolar = 6378137; + double radiusEqua = 6356752.314; + + 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); + + } + + /* @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 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; + + 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; + + } + + /* @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); + +// 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; + +//------------------------------------------------------------------------------------- +// 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; + + 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; + +//------------------------------------------------------------------------------------- +// 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); + + zeta *= arcsecFactor; + theta2 *= arcsecFactor; + ze *= arcsecFactor; + + 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); + +//------------------------------------------------------------------------------------- +// 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); + +// delta eps approx + 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;; + + 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 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); + +//------------------------------------------------------------------------------------- +// 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: "<