181 lines
6.3 KiB
C
181 lines
6.3 KiB
C
|
#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
||
|
|
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_
|
||
|
|
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <framework/coordinates/CoordinateTransformations.h>
|
||
|
|
#include <framework/globalfunctions/math/VectorOperations.h>
|
||
|
|
#include <framework/globalfunctions/timevalOperations.h>
|
||
|
|
#include <framework/globalfunctions/constants.h>
|
||
|
|
#include <memory.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_ */
|