eive-obsw/mission/controller/acs/Igrf13Model.cpp

190 lines
6.7 KiB
C++
Raw Normal View History

2022-09-19 10:46:21 +02:00
#include "Igrf13Model.h"
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <cmath>
2022-09-19 10:46:21 +02:00
#include "util/MathOperations.h"
2022-09-19 10:46:21 +02:00
using namespace Math;
Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {}
2022-09-19 10:46:21 +02:00
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) {
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * PI / 180;
theta *= (-1);
double rE = 6371200.0; // radius earth [m]
/* Predefine recursive associated Legendre polynomials */
double P11 = 1;
double P10 = P11; // P10 = P(n-1,m-0)
double dP11 = 0; // derivative
double dP10 = dP11; // derivative
double P2 = 0, dP2 = 0, P20 = 0, dP20 = 0, K = 0;
for (int m = 0; m <= igrfOrder; m++) {
for (int n = 1; n <= igrfOrder; n++) {
if (m <= n) {
/* Calculation of Legendre Polynoms (normalised) */
if (n == m) {
P2 = sin(theta) * P11;
dP2 = sin(theta) * dP11 + cos(theta) * P11;
P11 = P2;
P10 = P11;
P20 = 0;
dP11 = dP2;
dP10 = dP11;
dP20 = 0;
} else if (n == 1) {
P2 = cos(theta) * P10;
dP2 = cos(theta) * dP10 - sin(theta) * P10;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
} else {
K = (pow((n - 1), 2) - pow(m, 2)) / ((2 * n - 1) * (2 * n - 3));
P2 = cos(theta) * P10 - K * P20;
dP2 = cos(theta) * dP10 - sin(theta) * P10 - K * dP20;
P20 = P10;
P10 = P2;
dP20 = dP10;
dP10 = dP2;
}
/* gradient of scalar potential towards radius */
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 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 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);
}
}
}
magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta));
// 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 JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
// JD2000Floor = floor(JD2000);
// double JC2000 = JD2000Floor / 36525.;
double JD2000 = MathOperations<double>::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.;
// 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(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<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
2022-09-19 10:46:21 +02:00
}
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<double>::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000 - JD2000Igrf);
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {
updatedG[i][j] = coeffG[i][j] + svG[i][j] * (days / 365);
updatedH[i][j] = coeffH[i][j] + svH[i][j] * (days / 365);
}
}
2022-09-19 10:46:21 +02:00
}
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];
}
}
}