From 236ca64de3bbc47a9fe1848820b83efb382bd56f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 12 Feb 2024 14:43:34 +0100 Subject: [PATCH] whole lot of cleanup --- mission/controller/acs/AcsParameters.cpp | 12 +- mission/controller/acs/AttitudeEstimation.cpp | 4 +- mission/controller/acs/Guidance.cpp | 4 +- mission/controller/acs/Guidance.h | 1 - mission/controller/acs/Igrf13Model.cpp | 22 +- mission/controller/acs/Igrf13Model.h | 9 +- .../acs/MultiplicativeKalmanFilter.cpp | 36 +- mission/controller/acs/Navigation.cpp | 3 - mission/controller/acs/SensorProcessing.cpp | 8 +- mission/controller/acs/SensorProcessing.h | 3 +- mission/controller/acs/util/MathOperations.h | 478 ------------------ 11 files changed, 42 insertions(+), 538 deletions(-) delete mode 100644 mission/controller/acs/util/MathOperations.h diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 8816eed9..f8fb759b 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x11): // KalmanFilterParameters switch (parameterId) { case 0x0: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr); break; case 0x1: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus); break; case 0x2: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm); break; case 0x3: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr); break; case 0x4: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw); break; case 0x5: - parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR); + parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 3e4a22c4..3b6bf181 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, // Sensor Weights double kSus = 0, kMgm = 0; - kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2); - kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2); + kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2); + kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2); // Weighted Vectors double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0}, diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index ef21f7fc..b167573a 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -44,7 +44,7 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double targetF[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( + CoordinateTransformations::cartesianFromLatLongAlt( acsParameters->targetModeControllerParameters.latitudeTgt, acsParameters->targetModeControllerParameters.longitudeTgt, acsParameters->targetModeControllerParameters.altitudeTgt, targetF); @@ -98,7 +98,7 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou //------------------------------------------------------------------------------------- // transform longitude, latitude and altitude to cartesian coordiantes (ECEF) double posGroundStationF[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt( + CoordinateTransformations::cartesianFromLatLongAlt( acsParameters->gsTargetModeControllerParameters.latitudeTgt, acsParameters->gsTargetModeControllerParameters.longitudeTgt, acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF); diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 9835b762..c9ae1b07 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 3f844434..4ae7c93a 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -1,19 +1,5 @@ #include "Igrf13Model.h" -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "util/MathOperations.h" - -using namespace Math; - Igrf13Model::Igrf13Model() {} Igrf13Model::~Igrf13Model() {} @@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double magFieldModel[3] = {0, 0, 0}; double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ - theta -= 90 * PI / 180; + theta -= 90. * M_PI / 180.; theta *= (-1); double rE = 6371200.0; // radius earth [m] @@ -83,13 +69,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement); double UT1 = JD2000 / 36525.; double gst = 280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3); gst = std::fmod(gst, 360.); - gst *= PI / 180.; + gst *= M_PI / 180.; double lst = gst + longitude; // local sidereal time [rad] magFieldModelInertial[0] = @@ -107,7 +93,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000 - double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement); double days = ceil(JD2000 - JD2000Igrf); for (int i = 0; i <= igrfOrder; i++) { for (int j = 0; j <= (igrfOrder - 1); j++) { diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 187adde7..774d6c58 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,10 +16,11 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include -#include -#include -#include +#include +#include +#include +#include +#include #include diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index ba878034..efa74568 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -9,9 +9,6 @@ #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {} MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {} @@ -25,9 +22,9 @@ ReturnValue_t MultiplicativeKalmanFilter::init( if (validMagField_ && validSS && validSSModel && validMagModel) { // QUEST ALGO ----------------------------------------------------------------------- double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; - sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; + sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -234,9 +231,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // If we are here, MEKF will perform double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0; - sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS; - sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG; - sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR; + sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus; + sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm; + sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr; double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0}, normSunJ[3] = {0, 0, 0}; @@ -264,8 +261,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str - MathOperations::skewMatrix(sunEstB, *measSensMatrix11); - MathOperations::skewMatrix(magEstB, *measSensMatrix22); + MatrixOperations::skewMatrix(sunEstB, *measSensMatrix11); + MatrixOperations::skewMatrix(magEstB, *measSensMatrix22); double measVecQuat[3] = {0, 0, 0}; if (validSTR_) { @@ -837,8 +834,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF); // <> double invResidualCov[MDF][MDF] = {{0}}; - int inversionFailed = MathOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); - if (inversionFailed) { + ReturnValue_t result = + MatrixOperations::inverseMatrix(*residualCov, *invResidualCov, MDF); + if (result != returnvalue::OK) { updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED); return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed } @@ -874,7 +872,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( // State Vector Elements double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::skewMatrix(propagatedQuaternion, *xi2); + MatrixOperations::skewMatrix(propagatedQuaternion, *xi2); double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; MatrixOperations::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3); MatrixOperations::add(*xi1, *xi2, *xi1, 3, 3); @@ -898,8 +896,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( biasGYR[2] = updatedGyroBias[2]; /* ----------- PROPAGATION ----------*/ - double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR; - double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR; + double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs; + double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw; double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0}, {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}}; @@ -1057,7 +1055,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( VectorOperations::mulScalar(rotRateEst, sinFac, rotSin, 3); double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::skewMatrix(rotSin, *skewSin); + MatrixOperations::skewMatrix(rotSin, *skewSin); MatrixOperations::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3); double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -1080,8 +1078,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); - if (not(MathOperations::checkVectorIsFinite(propagatedQuaternion, 4)) || - not(MathOperations::checkMatrixIsFinite(initialQuaternion, 6, 6))) { + if (not(VectorOperations::isFinite(propagatedQuaternion, 4)) || + not(MatrixOperations::isFinite(initialQuaternion, 6, 6))) { updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); return MEKF_NOT_FINITE; } diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 624f4cea..5990eca1 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -5,9 +5,6 @@ #include #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - Navigation::Navigation() {} Navigation::~Navigation() {} diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 43582f18..61292c36 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -180,7 +180,7 @@ void SensorProcessing::processSus( const AcsParameters::SunModelParameters *sunModelParameters, acsctrl::SusDataProcessed *susDataProcessed) { /* -------- Sun Model Direction (IJK frame) ------- */ - double JD2000 = MathOperations::convertUnixToJD2000(timeAbsolute); + double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute); // Julean Centuries double sunIjkModel[3] = {0.0, 0.0, 0.0}; @@ -528,8 +528,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong uint8_t gpsSource = acs::gps::Source::NONE; // We do not trust the GPS and therefore it shall die here if SPG4 is running if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { - MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, - gdLongitude, altitude); + CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, + gdLatitude, gdLongitude, altitude); double factor = 1 - pow(ECCENTRICITY_WGS84, 2); gcLatitude = atan(factor * tan(gdLatitude)); { @@ -559,7 +559,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; - MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); + CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); VectorOperations::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3); diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index fea0fd01..35d3ae6d 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -2,11 +2,13 @@ #define SENSORPROCESSING_H_ #include +#include #include #include #include #include #include +#include #include #include #include @@ -14,7 +16,6 @@ #include #include #include -#include #include #include diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h deleted file mode 100644 index 7bf7319b..00000000 --- a/mission/controller/acs/util/MathOperations.h +++ /dev/null @@ -1,478 +0,0 @@ -#ifndef MATH_MATHOPERATIONS_H_ -#define MATH_MATHOPERATIONS_H_ - -#include -#include - -#include - -// Most of these functions already exist within the FSFW and are redundant -// All others should have been merged into the FSFW -// So if you are that bored, that you are reading this, you were better of merging these now - -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; - std::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) { - /* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude, - * longitude and altitude - * @param: lat geodetic latitude [rad] - * longi longitude [rad] - * alt altitude [m] - * cartesianOutput Cartesian Coordinates in ECEF (3x1) - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff - * Landis Markley and John L. Crassidis*/ - double radiusPolar = 6356752.314; - double radiusEqua = 6378137; - - 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 latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) { - /* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from - * cartesian coordinates in ECEF - * @param: x x-value of position vector [m] - * y y-value of position vector [m] - * z z-value of position vector [m] - * latitude geodetic latitude [rad] - * longitude longitude [rad] - * altitude altitude [m] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f - * Landis Markley and John L. Crassidis*/ - // From World Geodetic System the Earth Radii - double a = 6378137.0; // semimajor axis [m] - double b = 6356752.3142; // semiminor axis [m] - - // Calculation - double e2 = 1 - pow(b, 2) / pow(a, 2); - double epsilon2 = pow(a, 2) / pow(b, 2) - 1; - double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2)); - double p = std::abs(vector[2]) / epsilon2; - double s = pow(rho, 2) / (e2 * epsilon2); - double q = pow(p, 2) - pow(b, 2) + s; - double u = p / sqrt(q); - double v = pow(b, 2) * pow(u, 2) / q; - double P = 27 * v * s / q; - double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.); - double t = (1 + Q + 1 / Q) / 6; - double c = sqrt(pow(u, 2) - 1 + 2 * t); - double w = (c - u) / 2; - double d = - sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.)); - double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2)); - latitude = asin((epsilon2 + 1) * d / N); - altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N; - longitude = atan2(vector[1], vector[0]); - } - - static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) { - /* @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] - * outputDotDcmEJ Derivative of transformation matrix [3][3] - * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff - * Landis Markley and John L. Crassidis*/ - 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 * M_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; - - // Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION - double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]}, - {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]}, - {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}}; - double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}}; - double omegaEarth = 0.000072921158553; - double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3); - MatrixOperations::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 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) { - // 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 * M_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 * M_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 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_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 = 0; - 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 ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) { - // Stopwatch stopwatch; - T1 matrix[size][size], identity[size][size]; - // 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 - std::memset(identity, 0.0, sizeof(identity)); - for (uint8_t diag = 0; diag < size; diag++) { - identity[diag][diag] = 1; - } - // gauss-jordan algo - // sort matrix such as no diag entry shall be 0 - for (uint8_t row = 0; row < size; row++) { - if (matrix[row][row] == 0.0) { - bool swaped = false; - uint8_t rowIndex = 0; - while ((rowIndex < size) && !swaped) { - if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) { - 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; - } - rowIndex++; - } - if (!swaped) { - return returnvalue::FAILED; // matrix not invertible - } - } - } - - 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; - } - for (uint8_t colIndex = 0; colIndex < size; colIndex++) { - std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]); - std::swap(identity[row][colIndex], identity[rowIndex][colIndex]); - } - row--; - if (row < 0) { - return returnvalue::FAILED; // Matrix is not invertible - } - } - } - // remove non diag elements in matrix (jordan) - for (int row = 0; row < size; row++) { - for (int rowIndex = 0; rowIndex < size; rowIndex++) { - if (row != rowIndex) { - double ratio = matrix[rowIndex][row] / matrix[row][row]; - for (int colIndex = 0; colIndex < size; colIndex++) { - matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex]; - identity[rowIndex][colIndex] -= ratio * identity[row][colIndex]; - } - } - } - } - // normalize rows in matrix (gauss) - for (int row = 0; row < size; row++) { - for (int col = 0; col < size; col++) { - identity[row][col] = identity[row][col] / matrix[row][row]; - } - } - std::memcpy(inverse, identity, sizeof(identity)); - return returnvalue::OK; // successful inversion - } - - static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) { - for (uint8_t i = 0; i < size; i++) { - if (not std::isfinite(inputVector[i])) { - return false; - } - } - return true; - } - - static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) { - for (uint8_t col = 0; col < cols; col++) { - for (uint8_t row = 0; row < rows; row++) { - if (not std::isfinite(inputMatrix[row * cols + col])) { - return false; - } - } - } - return true; - } - - static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols, - uint8_t mainRows, uint8_t mainCols, uint8_t startRow, - uint8_t startCol) { - if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) { - return; - } - for (uint8_t row = 0; row < subRows; row++) { - for (uint8_t col = 0; col < subCols; col++) { - mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col]; - } - } - } -}; - -#endif /* ACS_MATH_MATHOPERATIONS_H_ */