added Igrf13Model

mission/controller/acs/Igrf13Model.cpp

@@ -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);
+		}
+	}
+}