diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index fcd95b68..ec1b3e08 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -1,22 +1,19 @@ -/* - * Igrf13Model.cpp - * - * Created on: 10 Mar 2022 - * Author: Robin Marquardt - */ - #include "Igrf13Model.h" -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include +#include + +#include #include "util/MathOperations.h" +using namespace Math; + Igrf13Model::Igrf13Model() {} Igrf13Model::~Igrf13Model() {} @@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, double* magFieldModelInertial) { double phi = longitude, theta = gcLatitude; // geocentric /* Here is the co-latitude needed*/ - theta -= 90 * Math::PI / 180; + theta -= 90 * PI / 180; theta *= (-1); double rE = 6371200.0; // radius earth [m] @@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, /* Calculation of Legendre Polynoms (normalised) */ if (n == m) { P2 = sin(theta) * P11; - dP2 = sin(theta) * dP11 - cos(theta) * P11; + dP2 = sin(theta) * dP11 + cos(theta) * P11; P11 = P2; P10 = P11; P20 = 0; @@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[0] += pow(rE / (altitude + rE), (n + 2)) * (n + 1) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2); - /* gradient of scalar potential towards phi */ + /* gradient of scalar potential towards theta */ magFieldModel[1] += pow(rE / (altitude + rE), (n + 2)) * ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2); - /* gradient of scalar potential towards theta */ + /* gradient of scalar potential towards phi */ magFieldModel[2] += pow(rE / (altitude + rE), (n + 2)) * ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m); @@ -85,31 +82,51 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, magFieldModel[1] *= -1; magFieldModel[2] *= (-1 / sin(theta)); - /* Next step: transform into inertial KOS (IJK)*/ + // std::cout << " X=" << -magFieldModel[1] << " Y=" << magFieldModel[2] + // << " Z=" << -magFieldModel[0] << std::endl; + + /* Next step: transform into inertial RF (IJK)*/ // Julean Centuries - double JD2000Floor = 0; + // double JD2000Floor = 0; + // double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); + // JD2000Floor = floor(JD2000); + // double JC2000 = JD2000Floor / 36525.; + double JD2000 = MathOperations::convertUnixToJD2000(timeOfMagMeasurement); - JD2000Floor = floor(JD2000); - double JC2000 = JD2000Floor / 36525; + double UT1 = JD2000 / 36525.; - double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - - 0.000000026 * pow(JC2000, 3); // greenwich sidereal time - gst *= PI / 180; // convert to radians - double sec = - (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? - double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] - gst += omega0 * sec; + 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.; + // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; + // double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) - + // 0.000000026 * pow(JC2000, 3); // greenwich sidereal time + // gst *= PI / 180; // convert to radians + // double sec = + // (JD2000 - JD2000Floor) * 86400; // Seconds on this day (Universal time) // FROM GPS ? + // double omega0 = 0.00007292115; // mean angular velocity earth [rad/s] + // gst += omega0 * sec; + // std::cout << " GMST=" << gst * 180. / Math::PI << std::endl; double lst = gst + longitude; // local sidereal time [rad] + // std::cout << " LMST=" << lst * 180. / Math::PI << std::endl; - magFieldModelInertial[0] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst); - magFieldModelInertial[1] = magFieldModel[0] * cos(theta) + - magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst); - magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst); + magFieldModelInertial[0] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) - + magFieldModel[2] * sin(lst); + magFieldModelInertial[1] = + (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) + + magFieldModel[2] * cos(lst); + magFieldModelInertial[2] = + magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude); double normVecMagFieldInert[3] = {0, 0, 0}; VectorOperations::normalize(magFieldModelInertial, normVecMagFieldInert, 3); + + magFieldModel[0] = 0; + magFieldModel[1] = 0; + magFieldModel[2] = 0; } void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { @@ -123,3 +140,50 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { } } } + +void Igrf13Model::schmidtNormalization() { + double kronDelta = 0; + schmidtFactors[0][0] = 1; + // for (int n = 1; n <= igrfOrder; n++) { + // if (n == 1) { + // schmidtFactors[0][n - 1] = 1; + // } else { + // schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n; + // } + // + // for (int m = 1; m <= igrfOrder; m++) { + // if (m == 1) { + // kronDelta = 1; + // } else { + // kronDelta = 0; + // } + // schmidtFactors[m][n - 1] = + // schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m)); + // } + // } + + for (int n = 1; n <= igrfOrder; n++) { + for (int m = 0; m <= n; m++) { + if (m > 1) { + schmidtFactors[m][n - 1] = schmidtFactors[m - 1][n - 1] * pow((n - m + 1) / (n + m), .5); + } else if (m > 0) { + schmidtFactors[m][n - 1] = + schmidtFactors[m - 1][n - 1] * pow(2 * (n - m + 1) / (n + m), .5); + } else if (n == 1) { + schmidtFactors[m][n - 1] = 1; + } else { + schmidtFactors[m][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / (n); + } + } + } + + for (int i = 0; i <= igrfOrder; i++) { + for (int j = 0; j <= (igrfOrder - 1); j++) { + coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j]; + coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j]; + + svG[i][j] = schmidtFactors[i][j] * svG[i][j]; + svH[i][j] = schmidtFactors[i][j] * svH[i][j]; + } + } +} diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index aa7e8b73..7abe97cc 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ { * - timeOfMagMeasurement: time of actual measurement [s] * * Outputs: - * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/ + * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/ // Coefficient wary over year, could be updated sometimes. void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) double magFieldModel[3]; + void schmidtNormalization(); private: - const double coeffG[14][13] = { + double coeffG[14][13] = { {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1}, {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9}, {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5}, @@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4}}; // [m][n] in nT - const double coeffH[14][13] = { + double coeffH[14][13] = { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0}, {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9}, {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6}, @@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.6}}; // [m][n] in nT - const double svG[14][13] = { + double svG[14][13] = { {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT - const double svH[14][13] = { + double svH[14][13] = { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0}, @@ -113,6 +114,23 @@ class Igrf13Model /*:public HasParametersIF*/ { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT + + double schmidtFactors[14][13] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}; + ; + bool schmidtNorm = false; double updatedG[14][13]; double updatedH[14][13];