126 lines
4.6 KiB
C++
126 lines
4.6 KiB
C++
/*
|
|
* Igrf13Model.cpp
|
|
*
|
|
* Created on: 10 Mar 2022
|
|
* Author: Robin Marquardt
|
|
*/
|
|
|
|
#include "Igrf13Model.h"
|
|
|
|
#include <fsfw/globalfunctions/constants.h>
|
|
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
|
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
|
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
|
|
#include "util/MathOperations.h"
|
|
|
|
Igrf13Model::Igrf13Model() {}
|
|
Igrf13Model::~Igrf13Model() {}
|
|
|
|
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 * Math::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 phi */
|
|
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 */
|
|
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));
|
|
|
|
/* Next step: transform into inertial KOS (IJK)*/
|
|
// Julean Centuries
|
|
double JD2000Floor = 0;
|
|
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
|
|
JD2000Floor = floor(JD2000);
|
|
double JC2000 = JD2000Floor / 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 lst = gst + longitude; // local sidereal time [rad]
|
|
|
|
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);
|
|
|
|
double normVecMagFieldInert[3] = {0, 0, 0};
|
|
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|