mission/controller/acs/Igrf13Model.cpp

@@ -1,22 +1,19 @@
-/*
- * Igrf13Model.cpp
- *
- *  Created on: 10 Mar 2022
- *      Author: Robin Marquardt
- */
-
 #include "Igrf13Model.h"
 
-#include <fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>
 #include <stdint.h>
 #include <string.h>
+#include <time.h>
+
+#include <cmath>
 
 #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<double>::convertUnixToJD2000(timeOfMagMeasurement);
+  //  JD2000Floor = floor(JD2000);
+  //  double JC2000 = JD2000Floor / 36525.;
+
   double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
-  JD2000Floor = floor(JD2000);
+  double UT1 = JD2000 / 36525.;
-  double JC2000 = JD2000Floor / 36525;
 
-  double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
+  double gst =
-               0.000000026 * pow(JC2000, 3);  // greenwich sidereal time
+      280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
-  gst *= PI / 180;                            // convert to radians
+  gst = std::fmod(gst, 360.);
-  double sec =
+  gst *= PI / 180.;
-      (JD2000 - JD2000Floor) * 86400;  // Seconds on this day (Universal time) // FROM GPS ?
+  //  std::cout << "  GMST=" << gst * 180. / Math::PI << std::endl;
-  double omega0 = 0.00007292115;       // mean angular velocity earth [rad/s]
-  gst += omega0 * sec;
 
+  //  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) +
+  magFieldModelInertial[0] =
-                             magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst);
+      (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
-  magFieldModelInertial[1] = magFieldModel[0] * cos(theta) +
+      magFieldModel[2] * sin(lst);
-                             magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst);
+  magFieldModelInertial[1] =
-  magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
+      (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;
 }
 
 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];
+    }
+  }
+}

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];