186 lines
7.6 KiB
C++
186 lines
7.6 KiB
C++
#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
|
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
|
|
|
#include <memory.h>
|
|
|
|
#include <cstdint>
|
|
|
|
#include "CoordinateTransformations.h"
|
|
#include "coordinatesConf.h"
|
|
#include "fsfw/globalfunctions/constants.h"
|
|
#include "fsfw/globalfunctions/math/VectorOperations.h"
|
|
#include "fsfw/globalfunctions/timevalOperations.h"
|
|
|
|
template <uint8_t DEGREE, uint8_t ORDER>
|
|
class Jgm3Model {
|
|
public:
|
|
static const uint32_t
|
|
factorialLookupTable[DEGREE + 3]; // This table is used instead of factorial calculation,
|
|
// must be increased if order or degree is higher
|
|
|
|
Jgm3Model() {
|
|
y0[0] = 0;
|
|
y0[1] = 0;
|
|
y0[2] = 0;
|
|
y0[3] = 0;
|
|
y0[4] = 0;
|
|
y0[5] = 0;
|
|
|
|
lastExecutionTime.tv_sec = 0;
|
|
lastExecutionTime.tv_usec = 0;
|
|
}
|
|
virtual ~Jgm3Model(){};
|
|
|
|
// double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime);
|
|
|
|
double y0[6]; // position and velocity at beginning of RK step in EC
|
|
timeval lastExecutionTime; // Time of last execution
|
|
|
|
void accelDegOrd(const double pos[3], const double S[ORDER + 1][DEGREE + 1],
|
|
const double C[ORDER + 1][DEGREE + 1], double* accel) {
|
|
// Get radius of this position
|
|
double r = VectorOperations<double>::norm(pos, 3);
|
|
|
|
// Initialize the V and W matrix
|
|
double V[DEGREE + 2][ORDER + 2] = {{0}};
|
|
double W[DEGREE + 2][ORDER + 2] = {{0}};
|
|
|
|
for (uint8_t m = 0; m < (ORDER + 2); m++) {
|
|
for (uint8_t n = m; n < (DEGREE + 2); n++) {
|
|
if ((n == 0) && (m == 0)) {
|
|
// Montenbruck "Satellite Orbits Eq.3.31"
|
|
V[0][0] = Earth::MEAN_RADIUS / r;
|
|
W[0][0] = 0;
|
|
} else {
|
|
if (n == m) {
|
|
// Montenbruck "Satellite Orbits Eq.3.29"
|
|
V[m][m] = (2 * m - 1) * (pos[0] * Earth::MEAN_RADIUS / pow(r, 2) * V[m - 1][m - 1] -
|
|
pos[1] * Earth::MEAN_RADIUS / pow(r, 2) * W[m - 1][m - 1]);
|
|
W[m][m] = (2 * m - 1) * (pos[0] * Earth::MEAN_RADIUS / pow(r, 2) * W[m - 1][m - 1] +
|
|
pos[1] * Earth::MEAN_RADIUS / pow(r, 2) * V[m - 1][m - 1]);
|
|
} else {
|
|
// Montenbruck "Satellite Orbits Eq.3.30"
|
|
V[n][m] = ((2 * n - 1) / (double)(n - m)) * pos[2] * Earth::MEAN_RADIUS / pow(r, 2) *
|
|
V[n - 1][m];
|
|
W[n][m] = ((2 * n - 1) / (double)(n - m)) * pos[2] * Earth::MEAN_RADIUS / pow(r, 2) *
|
|
W[n - 1][m];
|
|
if (n != (m + 1)) {
|
|
V[n][m] = V[n][m] - (((n + m - 1) / (double)(n - m)) *
|
|
(pow(Earth::MEAN_RADIUS, 2) / pow(r, 2)) * V[n - 2][m]);
|
|
W[n][m] = W[n][m] - (((n + m - 1) / (double)(n - m)) *
|
|
(pow(Earth::MEAN_RADIUS, 2) / pow(r, 2)) * W[n - 2][m]);
|
|
} // End of if(n!=(m+1))
|
|
} // End of if(n==m){
|
|
} // End of if(n==0 and m==0)
|
|
} // End of for(uint8_t n=0;n<(DEGREE+1);n++)
|
|
} // End of for(uint8_t m=0;m<(ORDER+1);m++)
|
|
|
|
// overwrite accel if not properly initialized
|
|
accel[0] = 0;
|
|
accel[1] = 0;
|
|
accel[2] = 0;
|
|
|
|
for (uint8_t m = 0; m < (ORDER + 1); m++) {
|
|
for (uint8_t n = m; n < (DEGREE + 1); n++) {
|
|
// Use table lookup to get factorial
|
|
double partAccel[3] = {0};
|
|
double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER / pow(Earth::MEAN_RADIUS, 2);
|
|
if (m == 0) {
|
|
// Montenbruck "Satellite Orbits Eq.3.33"
|
|
partAccel[0] = factor * (-C[n][0] * V[n + 1][1]);
|
|
partAccel[1] = factor * (-C[n][0] * W[n + 1][1]);
|
|
} else {
|
|
double factMN = static_cast<double>(factorialLookupTable[n - m + 2]) /
|
|
static_cast<double>(factorialLookupTable[n - m]);
|
|
partAccel[0] = factor * 0.5 *
|
|
((-C[n][m] * V[n + 1][m + 1] - S[n][m] * W[n + 1][m + 1]) +
|
|
factMN * (C[n][m] * V[n + 1][m - 1] + S[n][m] * W[n + 1][m - 1]));
|
|
partAccel[1] = factor * 0.5 *
|
|
((-C[n][m] * W[n + 1][m + 1] + S[n][m] * V[n + 1][m + 1]) +
|
|
factMN * (-C[n][m] * W[n + 1][m - 1] + S[n][m] * V[n + 1][m - 1]));
|
|
}
|
|
|
|
partAccel[2] = factor * ((n - m + 1) * (-C[n][m] * V[n + 1][m] - S[n][m] * W[n + 1][m]));
|
|
|
|
accel[0] += partAccel[0];
|
|
accel[1] += partAccel[1];
|
|
accel[2] += partAccel[2];
|
|
} // End of for(uint8_t n=0;n<DEGREE;n++)
|
|
} // End of uint8_t m=0;m<ORDER;m++
|
|
}
|
|
|
|
void initializeNavOrbit(const double position[3], const double velocity[3], timeval timeUTC) {
|
|
CoordinateTransformations::positionEcfToEci(position, &y0[0], &timeUTC);
|
|
CoordinateTransformations::velocityEcfToEci(velocity, position, &y0[3], &timeUTC);
|
|
lastExecutionTime = timeUTC;
|
|
}
|
|
|
|
void acsNavOrbit(timeval timeUTC, const double S[ORDER + 1][DEGREE + 1],
|
|
const double C[ORDER + 1][DEGREE + 1], double outputPos[3],
|
|
double outputVel[3]) {
|
|
// RK4 Integration for this timestamp
|
|
double deltaT = timevalOperations::toDouble(timeUTC - lastExecutionTime);
|
|
|
|
double y0dot[6] = {0, 0, 0, 0, 0, 0};
|
|
double yA[6] = {0, 0, 0, 0, 0, 0};
|
|
double yAdot[6] = {0, 0, 0, 0, 0, 0};
|
|
double yB[6] = {0, 0, 0, 0, 0, 0};
|
|
double yBdot[6] = {0, 0, 0, 0, 0, 0};
|
|
double yC[6] = {0, 0, 0, 0, 0, 0};
|
|
double yCdot[6] = {0, 0, 0, 0, 0, 0};
|
|
|
|
// Step One
|
|
rungeKuttaStep(y0, y0dot, lastExecutionTime, S, C);
|
|
|
|
// Step Two
|
|
VectorOperations<double>::mulScalar(y0dot, deltaT / 2, yA, 6);
|
|
VectorOperations<double>::add(y0, yA, yA, 6);
|
|
rungeKuttaStep(yA, yAdot, lastExecutionTime, S, C);
|
|
|
|
// Step Three
|
|
VectorOperations<double>::mulScalar(yAdot, deltaT / 2, yB, 6);
|
|
VectorOperations<double>::add(y0, yB, yB, 6);
|
|
rungeKuttaStep(yB, yBdot, lastExecutionTime, S, C);
|
|
|
|
// Step Four
|
|
VectorOperations<double>::mulScalar(yBdot, deltaT, yC, 6);
|
|
VectorOperations<double>::add(y0, yC, yC, 6);
|
|
rungeKuttaStep(yC, yCdot, lastExecutionTime, S, C);
|
|
|
|
// Calc new State
|
|
VectorOperations<double>::mulScalar(yAdot, 2, yAdot, 6);
|
|
VectorOperations<double>::mulScalar(yBdot, 2, yBdot, 6);
|
|
VectorOperations<double>::add(y0dot, yAdot, y0dot, 6);
|
|
VectorOperations<double>::add(y0dot, yBdot, y0dot, 6);
|
|
VectorOperations<double>::add(y0dot, yCdot, y0dot, 6);
|
|
VectorOperations<double>::mulScalar(y0dot, 1. / 6. * deltaT, y0dot, 6);
|
|
VectorOperations<double>::add(y0, y0dot, y0, 6);
|
|
|
|
CoordinateTransformations::positionEciToEcf(&y0[0], outputPos, &timeUTC);
|
|
CoordinateTransformations::velocityEciToEcf(&y0[3], &y0[0], outputVel, &timeUTC);
|
|
|
|
lastExecutionTime = timeUTC;
|
|
}
|
|
|
|
void rungeKuttaStep(const double* yIn, double* yOut, timeval time,
|
|
const double S[ORDER + 1][DEGREE + 1],
|
|
const double C[ORDER + 1][DEGREE + 1]) {
|
|
double rECF[3] = {0, 0, 0};
|
|
double rDotECF[3] = {0, 0, 0};
|
|
double accelECF[3] = {0, 0, 0};
|
|
double accelECI[3] = {0, 0, 0};
|
|
|
|
CoordinateTransformations::positionEciToEcf(&yIn[0], rECF, &time);
|
|
CoordinateTransformations::velocityEciToEcf(&yIn[3], &yIn[0], rDotECF, &time);
|
|
accelDegOrd(rECF, S, C, accelECF);
|
|
// This is not correct, as the acceleration would have derived terms but we don't know the
|
|
// velocity and position at that time Tests showed that a wrong velocity does make the equation
|
|
// worse than neglecting it
|
|
CoordinateTransformations::positionEcfToEci(accelECF, accelECI, &time);
|
|
memcpy(&yOut[0], &yIn[3], sizeof(yOut[0]) * 3);
|
|
memcpy(&yOut[3], accelECI, sizeof(yOut[0]) * 3);
|
|
}
|
|
};
|
|
|
|
#endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */
|