First Version of ACS Controller #329
126
mission/controller/acs/Igrf13Model.cpp
Normal file
126
mission/controller/acs/Igrf13Model.cpp
Normal file
@ -0,0 +1,126 @@
|
|||||||
|
/*
|
||||||
|
* Igrf13Model.cpp
|
||||||
|
*
|
||||||
|
* Created on: 10 Mar 2022
|
||||||
|
* Author: Robin Marquardt
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Igrf13Model.h"
|
||||||
|
#include <cmath>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <time.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 <acs/math/MathOperations.h>
|
||||||
|
|
||||||
|
using namespace Math;
|
||||||
|
|
||||||
|
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*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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
117
mission/controller/acs/Igrf13Model.h
Normal file
117
mission/controller/acs/Igrf13Model.h
Normal file
@ -0,0 +1,117 @@
|
|||||||
|
/*
|
||||||
|
* Igrf13Model.h
|
||||||
|
*
|
||||||
|
* Created on: 10 Mar 2022
|
||||||
|
* Author: Robin Marquardt
|
||||||
|
* Description: Calculates the magnetic field vector of earth with the IGRF Model.
|
||||||
|
* Sources: https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html
|
||||||
|
* https://doi.org/10.1186/s40623-020-01288-x
|
||||||
|
* J. Davis, Mathematical Modeling of Earth's Magnetic Field, TN, 2004
|
||||||
|
*
|
||||||
|
* [Conversion of ENU (geocentric) to IJK: Skript Bahnmechanik für Raumfahrzeuge,
|
||||||
|
* Prof. Dr.-Ing. Stefanos Fasoulas / Dr.-Ing. Frank Zimmermann]
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef IGRF13MODEL_H_
|
||||||
|
#define IGRF13MODEL_H_
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
|
||||||
|
// Output should be transformed to [T] instead of [nT]
|
||||||
|
// Updating Coefficients has to be implemented yet. Question, updating everyday ?
|
||||||
|
class Igrf13Model {
|
||||||
|
|
||||||
|
public:
|
||||||
|
Igrf13Model();
|
||||||
|
virtual ~Igrf13Model();
|
||||||
|
|
||||||
|
// Main Function
|
||||||
|
void magFieldComp(const double longitude, const double gcLatitude, const double altitude, timeval timeOfMagMeasurement,double* magFieldModelInertial);
|
||||||
|
// Right now the radius for igrf is simply with r0 + altitude calculated. In reality the radius is oriented from the satellite to earth COM
|
||||||
|
// Difference up to 25 km, which is 5 % of the total flight altitude
|
||||||
|
/* Inputs:
|
||||||
|
* - longitude: geocentric longitude [rad]
|
||||||
|
* - latitude: geocentric latitude [rad]
|
||||||
|
* - altitude: [m]
|
||||||
|
* - timeOfMagMeasurement: time of actual measurement [s]
|
||||||
|
*
|
||||||
|
* Outputs:
|
||||||
|
* - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
|
||||||
|
|
||||||
|
|
||||||
|
// Coefficient wary over year, could be updated sometimes.
|
||||||
|
void updateCoeffGH(timeval timeOfMagMeasurement); //Secular variation (SV)
|
||||||
|
double magFieldModel[3];
|
||||||
|
|
||||||
|
private:
|
||||||
|
const 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, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
|
||||||
|
{0, 0, 525.7, -309.4, -140.7, -121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3 ,0.7},
|
||||||
|
{0 ,0 ,0, 48.0, -151.2, -36.2, 15.8, -21.1, -1.1, -0.9, -0.9, -1.2, -0.3},
|
||||||
|
{0, 0, 0, 0, 13.5, 13.5, 6.4, 15.3, -13.2, 0.7, 0.3, 0.7, 0.8},
|
||||||
|
{0, 0, 0, 0, 0,-64.7, -7.2, 13.7, 1.1, -0.9, -0.7, 0.3, 0.0},
|
||||||
|
{0, 0, 0, 0, 0, 0, 9.8, -16.5, 8.8, 1.9, -0.1, 0.5, 0.8},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, -0.3, -9.3, 1.4, 1.4, -0.3, 0.0},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, -11.9, -2.4, -0.6, -0.5, 0.4},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, 0, -3.8, 0.2, 0.1, 0.1},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.1, -1.1, 0.5},
|
||||||
|
{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.4}}; // [m][n] in nT
|
||||||
|
|
||||||
|
const double coeffH[14][13] = {{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, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
|
||||||
|
{0, 0, -543.4, 199.7, -121.2, 52.8, 2.2, 12.8, 9.8, 3.6, -0.6, 1.4, 1.4},
|
||||||
|
{0, 0, 0, -349.7, 32.3, -64.5, 23.5, -11.7, -5.1, 4.8, -0.4, -1.8, -0.4},
|
||||||
|
{0, 0, 0, 0, 98.9, 8.9, -2.2, 14.9, -6.3, -8.6, 0.6, 0.1, -1.3},
|
||||||
|
{0, 0, 0, 0, 0, 68.1, -27.2, 3.6, 7.8, -0.1, -0.2, 0.8, -0.1},
|
||||||
|
{0, 0, 0, 0, 0, 0, -1.8, -6.9, 0.4, -4.3, -1.7, -0.2, 0.3},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 2.8, -1.4, -3.4, -1.6, 0.6, -0.1},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, 9.6, -0.1, -3.0, 0.2, 0.5},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, 0, -8.8, -2.0, -0.9, 0.5},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2.6, 0.0, -0.4},
|
||||||
|
{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.6}}; // [m][n] in nT
|
||||||
|
|
||||||
|
const double svG[14][13] = {{5.7, -11, 2.2, -1.2, -0.3, -0.5, -0.1, 0, 0, 0, 0, 0 ,0},
|
||||||
|
{7.4, -7, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0, 0, 0, 0,0},
|
||||||
|
{0, -2.1, 3.1, -5.9, -0.6, 0.4, 0, -0.1, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, -12, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0},
|
||||||
|
{0 ,0 ,0, -5.1, 1.3, -1.4, 0.1, -0.1, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0.9, 0, -0.5, 0.4, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0, 0.9, -0.8, 0.3, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0.8, -0.1, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0, 0, 0, 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, 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] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
|
||||||
|
{-25.9, -30.2, 6, -0.1, 0, 0, 0.6, -0.2, 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.5, 3.6, -0.6, -1.3, -0.8, -0.2, 0, 0, 0, 0, 0},
|
||||||
|
{0 ,0 ,0, -5, 3, 0.8, -0.2, 0.5, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0.3, 0, -1.1, -0.3, 0, 0, 0, 0, 0},
|
||||||
|
{0, 0, 0, 0, 0, 1, 0.1, -0.4, 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, 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 updatedG[14][13];
|
||||||
|
double updatedH[14][13];
|
||||||
|
static const int igrfOrder = 13; // degree of truncation
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* IGRF13MODEL_H_ */
|
||||||
Loading…
Reference in New Issue
Block a user