Merge branch 'main' into str-secondary-fw-slot-update
Some checks are pending
EIVE/eive-obsw/pipeline/pr-main Build started...
Some checks are pending
EIVE/eive-obsw/pipeline/pr-main Build started...
This commit is contained in:
commit
a7b05f60a7
@ -37,6 +37,11 @@ will consitute of a breaking change warranting a new major release:
|
||||
- Changed PTG Strat priorities to favor STR before MEKF.
|
||||
- Increased message queue depth and maximum number of handled messages per cycle for
|
||||
`PusServiceBase` based classes (especially PUS scheduler).
|
||||
- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw`
|
||||
- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This
|
||||
also limits the rotation for the reference target quaternion to prevent spikes in required
|
||||
rotation rates.
|
||||
- Updated QUEST and Sun Vector Params to new values.
|
||||
|
||||
## Added
|
||||
|
||||
|
||||
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit 0d4a862c1af78ee5568b3268afc526be70fa055b
|
||||
Subproject commit 516357d855c07786b492e981230988186376d301
|
||||
@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
case (0x11): // KalmanFilterParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
|
||||
break;
|
||||
case 0x1:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
|
||||
@ -8,6 +8,9 @@
|
||||
typedef unsigned char uint8_t;
|
||||
|
||||
class AcsParameters : public HasParametersIF {
|
||||
private:
|
||||
static constexpr double DEG2RAD = M_PI / 180.;
|
||||
|
||||
public:
|
||||
AcsParameters();
|
||||
virtual ~AcsParameters();
|
||||
@ -22,7 +25,7 @@ class AcsParameters : public HasParametersIF {
|
||||
uint8_t fusedRateSafeDuringEclipse = true;
|
||||
uint8_t fusedRateFromStr = true;
|
||||
uint8_t fusedRateFromQuest = true;
|
||||
double questFilterWeight = 0.0;
|
||||
double questFilterWeight = 0.9;
|
||||
} onBoardParams;
|
||||
|
||||
struct InertiaEIVE {
|
||||
@ -773,7 +776,7 @@ class AcsParameters : public HasParametersIF {
|
||||
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
|
||||
-0.000889232196185857, -0.00168429567131815}};
|
||||
float susBrightnessThreshold = 0.7;
|
||||
float susVectorFilterWeight = .85;
|
||||
float susVectorFilterWeight = .95;
|
||||
float susRateFilterWeight = .99;
|
||||
} susHandlingParameters;
|
||||
|
||||
@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF {
|
||||
struct PointingLawParameters {
|
||||
double zeta = 0.3;
|
||||
double om = 0.3;
|
||||
double omMax = 1 * M_PI / 180;
|
||||
double omMax = 1 * DEG2RAD;
|
||||
double qiMin = 0.1;
|
||||
|
||||
double gainNullspace = 0.01;
|
||||
@ -876,15 +879,15 @@ class AcsParameters : public HasParametersIF {
|
||||
uint8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
// Default is Stuttgart GS
|
||||
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
|
||||
// For one-axis control:
|
||||
uint8_t avoidBlindStr = true;
|
||||
double blindAvoidStart = 1.5;
|
||||
double blindAvoidStop = 2.5;
|
||||
double blindRotRate = 1 * M_PI / 180;
|
||||
double blindRotRate = 1. * DEG2RAD;
|
||||
} targetModeControllerParameters;
|
||||
|
||||
struct GsTargetModeControllerParameters : PointingLawParameters {
|
||||
@ -892,9 +895,9 @@ class AcsParameters : public HasParametersIF {
|
||||
uint8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
// Default is Stuttgart GS
|
||||
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
|
||||
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
|
||||
double altitudeTgt = 500; // [m]
|
||||
} gsTargetModeControllerParameters;
|
||||
|
||||
struct NadirModeControllerParameters : PointingLawParameters {
|
||||
@ -911,8 +914,8 @@ class AcsParameters : public HasParametersIF {
|
||||
} inertialModeControllerParameters;
|
||||
|
||||
struct StrParameters {
|
||||
double exclusionAngle = 20 * M_PI / 180;
|
||||
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
|
||||
double exclusionAngle = 20. * DEG2RAD;
|
||||
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf
|
||||
} strParameters;
|
||||
|
||||
struct GpsParameters {
|
||||
@ -925,25 +928,25 @@ class AcsParameters : public HasParametersIF {
|
||||
|
||||
struct SunModelParameters {
|
||||
float domega = 36000.771;
|
||||
float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of
|
||||
// perigee
|
||||
float m_0 = 357.5277; // coefficients for mean anomaly
|
||||
float dm = 35999.049; // coefficients for mean anomaly
|
||||
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis
|
||||
float e1 = 0.74508 * M_PI / 180.;
|
||||
float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of
|
||||
// perigee
|
||||
float m_0 = 357.5277; // coefficients for mean anomaly
|
||||
float dm = 35999.049; // coefficients for mean anomaly
|
||||
float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis
|
||||
float e1 = 0.74508 * DEG2RAD;
|
||||
|
||||
float p1 = 6892. / 3600. * M_PI / 180.; // some parameter
|
||||
float p2 = 72. / 3600. * M_PI / 180.; // some parameter
|
||||
float p1 = 6892. / 3600. * DEG2RAD; // some parameter
|
||||
float p2 = 72. / 3600. * DEG2RAD; // some parameter
|
||||
} sunModelParameters;
|
||||
|
||||
struct KalmanFilterParameters {
|
||||
double sensorNoiseSTR = 0.1 * M_PI / 180;
|
||||
double sensorNoiseSS = 8 * M_PI / 180;
|
||||
double sensorNoiseMAG = 4 * M_PI / 180;
|
||||
double sensorNoiseGYR = 0.1 * M_PI / 180;
|
||||
double sensorNoiseStr = 0.1 * DEG2RAD;
|
||||
double sensorNoiseSus = 8. * DEG2RAD;
|
||||
double sensorNoiseMgm = 4. * DEG2RAD;
|
||||
double sensorNoiseGyr = 0.1 * DEG2RAD;
|
||||
|
||||
double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
|
||||
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
|
||||
double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
|
||||
double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
|
||||
} kalmanFilterParameters;
|
||||
|
||||
struct MagnetorquerParameter {
|
||||
@ -959,8 +962,8 @@ class AcsParameters : public HasParametersIF {
|
||||
|
||||
struct DetumbleParameter {
|
||||
uint8_t detumblecounter = 75; // 30 s
|
||||
double omegaDetumbleStart = 2 * M_PI / 180;
|
||||
double omegaDetumbleEnd = 1 * M_PI / 180;
|
||||
double omegaDetumbleStart = 2 * DEG2RAD;
|
||||
double omegaDetumbleEnd = 1 * DEG2RAD;
|
||||
double gainBdot = pow(10.0, -3.3);
|
||||
double gainFull = pow(10.0, -2.3);
|
||||
uint8_t useFullDetumbleLaw = false;
|
||||
|
||||
@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
|
||||
|
||||
// Sensor Weights
|
||||
double kSus = 0, kMgm = 0;
|
||||
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
|
||||
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
|
||||
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
|
||||
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
|
||||
|
||||
// Weighted Vectors
|
||||
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
|
||||
|
||||
@ -8,8 +8,8 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
|
||||
const double sunDirI[3], const double posSatF[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxisXI, 3);
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);
|
||||
|
||||
// determine helper vector to point x-Axis and therefore the STR away from Earth
|
||||
double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
|
||||
@ -17,39 +17,37 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
|
||||
VectorOperations<double>::normalize(posSatI, helperXI, 3);
|
||||
|
||||
// construct y-axis from helper vector and z-axis
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisXI, helperXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// x-axis completes RHS
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxisXI, zAxisXI, xAxisXI);
|
||||
VectorOperations<double>::normalize(xAxisXI, xAxisXI, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
|
||||
VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
// calculate of reference rotation rate
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatF[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatF[3], const double velSatF[3],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetF[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(
|
||||
acsParameters->targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
|
||||
double targetDirF[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
|
||||
@ -59,8 +57,8 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
|
||||
|
||||
// x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxisXI, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
|
||||
|
||||
// transform velocity into inertial frame
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
@ -73,32 +71,32 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
|
||||
|
||||
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
|
||||
// resolution
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxisXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// z-axis completes RHS
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxisXI, yAxisXI, zAxisXI);
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatF[3], const double sunDirI[3],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for ground station pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double posGroundStationF[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(
|
||||
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
|
||||
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
|
||||
@ -106,43 +104,93 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
|
||||
CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
|
||||
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
|
||||
|
||||
// negative x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxisXI, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
||||
|
||||
// get sun vector model in ECI
|
||||
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
|
||||
// get earth vector in ECI
|
||||
double earthDirI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, earthDirI, 3);
|
||||
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
|
||||
|
||||
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
|
||||
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
|
||||
double xDotS = VectorOperations<double>::dot(xAxisXI, sunDirI);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxisXI, 3), 2);
|
||||
double sunParallel[3], zAxisXI[3];
|
||||
VectorOperations<double>::mulScalar(xAxisXI, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisXI, 3);
|
||||
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
|
||||
// sun avoidance calculations
|
||||
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
|
||||
sunPerpendicularX, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
|
||||
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
|
||||
VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
|
||||
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
|
||||
strVecSunZ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
||||
strVecSunX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
|
||||
strVecSunZ, 3);
|
||||
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
|
||||
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
|
||||
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
|
||||
|
||||
// y-axis completes RHS
|
||||
double yAxisXI[3];
|
||||
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
|
||||
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
|
||||
// earth avoidance calculations
|
||||
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
|
||||
earthPerpendicularX, 3);
|
||||
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
|
||||
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
|
||||
VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
|
||||
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
|
||||
strVecEarthZ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
|
||||
strVecEarthX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
|
||||
strVecEarthZ, 3);
|
||||
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
|
||||
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
|
||||
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
|
||||
|
||||
if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
|
||||
// if this actually ever happens i will eat a broom
|
||||
sunWeight = 0.5;
|
||||
earthWeight = 0.5;
|
||||
}
|
||||
|
||||
// normalize weights for convenience
|
||||
double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
|
||||
sunWeight *= normFactor;
|
||||
earthWeight *= normFactor;
|
||||
|
||||
// calculate z-axis for str blinding avoidance
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
|
||||
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
|
||||
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
||||
|
||||
// calculate y-axis
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
|
||||
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
limitReferenceRotation(xAxisIX, targetQuat);
|
||||
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
|
||||
|
||||
std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
|
||||
double velSatE[3], double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
|
||||
const double posSatE[3], const double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
@ -153,26 +201,26 @@ void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
|
||||
|
||||
// negative x-axis aligned with position vector
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxisXI, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
|
||||
double xAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
|
||||
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
|
||||
|
||||
// make z-Axis parallel to major part of camera resolution
|
||||
double zAxisXI[3] = {0, 0, 0};
|
||||
double zAxisIX[3] = {0, 0, 0};
|
||||
double velSatI[3] = {0, 0, 0};
|
||||
CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
|
||||
VectorOperations<double>::cross(xAxisXI, velSatI, zAxisXI);
|
||||
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
|
||||
VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
|
||||
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
|
||||
|
||||
// y-Axis completes RHS
|
||||
double yAxisXI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
|
||||
double yAxisIX[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
|
||||
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
|
||||
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
|
||||
QuaternionOperations::fromDcm(dcmXI, targetQuat);
|
||||
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
|
||||
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
|
||||
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
targetRotationRate(timeDelta, targetQuat, refSatRate);
|
||||
}
|
||||
@ -189,6 +237,59 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
|
||||
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
|
||||
}
|
||||
|
||||
void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
|
||||
if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
|
||||
(VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// check required rotation and return if below limit
|
||||
double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(quatIXprev, quatXprevI);
|
||||
QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
|
||||
QuaternionOperations::normalize(quatXprevX);
|
||||
double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
|
||||
acsParameters->onBoardParams.sampleTime;
|
||||
if (2 * std::acos(quatXprevX[3]) < phiMax) {
|
||||
return;
|
||||
}
|
||||
|
||||
// x-axis always needs full rotation
|
||||
double phiX = 0, phiXvec[3] = {0, 0, 0};
|
||||
phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
|
||||
VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
|
||||
VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
|
||||
|
||||
double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
|
||||
std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
|
||||
quatXprevXtilde[3] = cos(phiX / 2.);
|
||||
QuaternionOperations::normalize(quatXprevXtilde);
|
||||
QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
|
||||
|
||||
// use the residual rotation up to the maximum
|
||||
double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(quatIX, quatXI);
|
||||
QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
|
||||
|
||||
double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
|
||||
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
|
||||
std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
|
||||
VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
|
||||
|
||||
double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
|
||||
3);
|
||||
std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
|
||||
quatXhatXTilde[3] = std::cos(phiResidual / 2.);
|
||||
QuaternionOperations::normalize(quatXhatXTilde);
|
||||
|
||||
// calculate final quaternion
|
||||
QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
|
||||
QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
|
||||
QuaternionOperations::normalize(quatIX);
|
||||
}
|
||||
|
||||
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
|
||||
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
|
||||
@ -255,7 +356,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
|
||||
}
|
||||
|
||||
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
|
||||
void Guidance::resetValues() {
|
||||
std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
|
||||
std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
|
||||
}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
|
||||
std::error_code e;
|
||||
|
||||
@ -8,9 +8,7 @@
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/acs/util/MathOperations.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
@ -27,16 +25,18 @@ class Guidance {
|
||||
|
||||
void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
|
||||
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
|
||||
double velSatF[3], double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
|
||||
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
|
||||
const double velSatF[3], double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
|
||||
|
||||
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
|
||||
double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
|
||||
@ -54,6 +54,7 @@ class Guidance {
|
||||
|
||||
bool strBlindAvoidFlag = false;
|
||||
double quatIXprev[4] = {0, 0, 0, 0};
|
||||
double xAxisIXprev[3] = {0, 0, 0};
|
||||
|
||||
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
|
||||
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
|
||||
|
||||
@ -1,19 +1,5 @@
|
||||
#include "Igrf13Model.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 <stdint.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
using namespace Math;
|
||||
|
||||
Igrf13Model::Igrf13Model() {}
|
||||
Igrf13Model::~Igrf13Model() {}
|
||||
|
||||
@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
||||
double magFieldModel[3] = {0, 0, 0};
|
||||
double phi = longitude, theta = gcLatitude; // geocentric
|
||||
/* Here is the co-latitude needed*/
|
||||
theta -= 90 * PI / 180;
|
||||
theta -= 90. * M_PI / 180.;
|
||||
theta *= (-1);
|
||||
|
||||
double rE = 6371200.0; // radius earth [m]
|
||||
@ -83,13 +69,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
||||
magFieldModel[1] *= -1;
|
||||
magFieldModel[2] *= (-1 / sin(theta));
|
||||
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
|
||||
double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
|
||||
double UT1 = JD2000 / 36525.;
|
||||
|
||||
double gst =
|
||||
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
|
||||
gst = std::fmod(gst, 360.);
|
||||
gst *= PI / 180.;
|
||||
gst *= M_PI / 180.;
|
||||
double lst = gst + longitude; // local sidereal time [rad]
|
||||
|
||||
magFieldModelInertial[0] =
|
||||
@ -107,7 +93,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
|
||||
|
||||
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 JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
|
||||
double days = ceil(JD2000 - JD2000Igrf);
|
||||
for (int i = 0; i <= igrfOrder; i++) {
|
||||
for (int j = 0; j <= (igrfOrder - 1); j++) {
|
||||
|
||||
@ -16,10 +16,11 @@
|
||||
#ifndef IGRF13MODEL_H_
|
||||
#define IGRF13MODEL_H_
|
||||
|
||||
#include <fsfw/parameters/HasParametersIF.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/TimeSystems.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 <cmath>
|
||||
|
||||
|
||||
@ -9,9 +9,6 @@
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
|
||||
|
||||
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
|
||||
@ -25,9 +22,9 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
|
||||
if (validMagField_ && validSS && validSSModel && validMagModel) {
|
||||
// QUEST ALGO -----------------------------------------------------------------------
|
||||
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
|
||||
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
|
||||
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
|
||||
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
|
||||
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
|
||||
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
|
||||
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
|
||||
|
||||
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
|
||||
normSunJ[3] = {0, 0, 0};
|
||||
@ -234,9 +231,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
|
||||
// If we are here, MEKF will perform
|
||||
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
|
||||
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
|
||||
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
|
||||
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
|
||||
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
|
||||
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
|
||||
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
|
||||
|
||||
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
|
||||
normSunJ[3] = {0, 0, 0};
|
||||
@ -264,8 +261,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss
|
||||
double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag
|
||||
double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
|
||||
MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
|
||||
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
|
||||
MatrixOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
|
||||
MatrixOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
|
||||
|
||||
double measVecQuat[3] = {0, 0, 0};
|
||||
if (validSTR_) {
|
||||
@ -837,8 +834,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
|
||||
// <<INVERSE residualCov HIER>>
|
||||
double invResidualCov[MDF][MDF] = {{0}};
|
||||
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
|
||||
if (inversionFailed) {
|
||||
ReturnValue_t result =
|
||||
MatrixOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
|
||||
if (result != returnvalue::OK) {
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
|
||||
return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
|
||||
}
|
||||
@ -874,7 +872,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
// State Vector Elements
|
||||
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
|
||||
xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
|
||||
MatrixOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
|
||||
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
|
||||
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3);
|
||||
MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3);
|
||||
@ -898,8 +896,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
biasGYR[2] = updatedGyroBias[2];
|
||||
|
||||
/* ----------- PROPAGATION ----------*/
|
||||
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
|
||||
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
|
||||
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;
|
||||
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
|
||||
|
||||
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
|
||||
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
|
||||
@ -1057,7 +1055,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
|
||||
|
||||
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::skewMatrix(rotSin, *skewSin);
|
||||
MatrixOperations<double>::skewMatrix(rotSin, *skewSin);
|
||||
|
||||
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
|
||||
double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
@ -1080,8 +1078,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
|
||||
|
||||
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
|
||||
|
||||
if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
|
||||
not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
|
||||
if (not(VectorOperations<double>::isFinite(propagatedQuaternion, 4)) ||
|
||||
not(MatrixOperations<double>::isFinite(*initialCovarianceMatrix, 6, 6))) {
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
|
||||
return MEKF_NOT_FINITE;
|
||||
}
|
||||
|
||||
@ -5,9 +5,6 @@
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
Navigation::Navigation() {}
|
||||
|
||||
Navigation::~Navigation() {}
|
||||
|
||||
@ -180,7 +180,7 @@ void SensorProcessing::processSus(
|
||||
const AcsParameters::SunModelParameters *sunModelParameters,
|
||||
acsctrl::SusDataProcessed *susDataProcessed) {
|
||||
/* -------- Sun Model Direction (IJK frame) ------- */
|
||||
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
|
||||
double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute);
|
||||
|
||||
// Julean Centuries
|
||||
double sunIjkModel[3] = {0.0, 0.0, 0.0};
|
||||
@ -198,6 +198,7 @@ void SensorProcessing::processSus(
|
||||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
VectorOperations<double>::normalize(sunIjkModel, sunIjkModel, 3);
|
||||
|
||||
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
if (sus0valid) {
|
||||
@ -528,8 +529,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
||||
uint8_t gpsSource = acs::gps::Source::NONE;
|
||||
// We do not trust the GPS and therefore it shall die here if SPG4 is running
|
||||
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
|
||||
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
|
||||
gdLongitude, altitude);
|
||||
CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
|
||||
gdLatitude, gdLongitude, altitude);
|
||||
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
|
||||
gcLatitude = atan(factor * tan(gdLatitude));
|
||||
{
|
||||
@ -559,7 +560,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
||||
|
||||
// Calculation of the satellite velocity in earth fixed frame
|
||||
double deltaDistance[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
||||
CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
||||
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
|
||||
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
|
||||
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
|
||||
|
||||
@ -2,7 +2,9 @@
|
||||
#define SENSORPROCESSING_H_
|
||||
|
||||
#include <common/config/eive/resultClassIds.h>
|
||||
#include <fsfw/coordinates/CoordinateTransformations.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/TimeSystems.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
@ -14,7 +16,6 @@
|
||||
#include <mission/controller/acs/Igrf13Model.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/acs/SusConverter.h>
|
||||
#include <mission/controller/acs/util/MathOperations.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
@ -1,98 +0,0 @@
|
||||
/*
|
||||
* TinyEKF: Extended Kalman Filter for embedded processors
|
||||
*
|
||||
* Copyright (C) 2015 Simon D. Levy
|
||||
*
|
||||
* MIT License
|
||||
*/
|
||||
#ifndef CHOLESKYDECOMPOSITION_H_
|
||||
#define CHOLESKYDECOMPOSITION_H_
|
||||
#include <math.h>
|
||||
// typedef unsigned int uint8_t;
|
||||
|
||||
template <typename T1, typename T2 = T1, typename T3 = T2>
|
||||
class CholeskyDecomposition {
|
||||
public:
|
||||
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
return cholsl(matrix, result, tempMatrix, dimension);
|
||||
}
|
||||
|
||||
private:
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t choldc1(double *a, double *p, uint8_t n) {
|
||||
int8_t i, j, k;
|
||||
double sum;
|
||||
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = i; j < n; j++) {
|
||||
sum = a[i * n + j];
|
||||
for (k = i - 1; k >= 0; k--) {
|
||||
sum -= a[i * n + k] * a[j * n + k];
|
||||
}
|
||||
if (i == j) {
|
||||
if (sum <= 0) {
|
||||
return 1; /* error */
|
||||
}
|
||||
p[i] = sqrt(sum);
|
||||
} else {
|
||||
a[j * n + i] = sum / p[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
|
||||
uint8_t i, j, k;
|
||||
double sum;
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
|
||||
if (choldc1(a, p, n)) return 1;
|
||||
for (i = 0; i < n; i++) {
|
||||
a[i * n + i] = 1 / p[i];
|
||||
for (j = i + 1; j < n; j++) {
|
||||
sum = 0;
|
||||
for (k = i; k < j; k++) {
|
||||
sum -= a[j * n + k] * a[k * n + i];
|
||||
}
|
||||
a[j * n + i] = sum / p[j];
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
|
||||
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
|
||||
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
|
||||
uint8_t i, j, k;
|
||||
if (choldcsl(A, a, p, n)) return 1;
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = i + 1; j < n; j++) {
|
||||
a[i * n + j] = 0.0;
|
||||
}
|
||||
}
|
||||
for (i = 0; i < n; i++) {
|
||||
a[i * n + i] *= a[i * n + i];
|
||||
for (k = i + 1; k < n; k++) {
|
||||
a[i * n + i] += a[k * n + i] * a[k * n + i];
|
||||
}
|
||||
for (j = i + 1; j < n; j++) {
|
||||
for (k = j; k < n; k++) {
|
||||
a[i * n + j] += a[k * n + i] * a[k * n + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < n; i++) {
|
||||
for (j = 0; j < i; j++) {
|
||||
a[i * n + j] = a[j * n + i];
|
||||
}
|
||||
}
|
||||
|
||||
return 0; /* success */
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */
|
||||
@ -1,465 +0,0 @@
|
||||
#ifndef MATH_MATHOPERATIONS_H_
|
||||
#define MATH_MATHOPERATIONS_H_
|
||||
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/sign.h>
|
||||
#include <fsfw/src/fsfw/serviceinterface.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
|
||||
template <typename T1, typename T2 = T1>
|
||||
class MathOperations {
|
||||
public:
|
||||
static void skewMatrix(const T1 vector[], T2 *result) {
|
||||
// Input Dimension [3], Output [3][3]
|
||||
result[0] = 0;
|
||||
result[1] = -vector[2];
|
||||
result[2] = vector[1];
|
||||
result[3] = vector[2];
|
||||
result[4] = 0;
|
||||
result[5] = -vector[0];
|
||||
result[6] = -vector[1];
|
||||
result[7] = vector[0];
|
||||
result[8] = 0;
|
||||
}
|
||||
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
|
||||
uint8_t size = 3) {
|
||||
// Looks like MatrixOpertions::multiply is able to do the same thing
|
||||
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
|
||||
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
|
||||
result[resultColumn + size * resultRow] =
|
||||
vector1[resultRow] * transposeVector2[resultColumn];
|
||||
}
|
||||
}
|
||||
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
|
||||
matrixMag[i][j] = magEstB[i] * magEstB[j];
|
||||
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
|
||||
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
|
||||
}
|
||||
|
||||
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
|
||||
int min_idx;
|
||||
T1 temp;
|
||||
std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
|
||||
// One by one move boundary of unsorted subarray
|
||||
for (int k = 0; k < rowSize; k++) {
|
||||
for (int i = 0; i < colSize - 1; i++) {
|
||||
// Find the minimum element in unsorted array
|
||||
min_idx = i;
|
||||
for (int j = i + 1; j < colSize; j++) {
|
||||
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
|
||||
min_idx = j;
|
||||
}
|
||||
}
|
||||
// Swap the found minimum element with the first element
|
||||
temp = result[i + k * colSize];
|
||||
result[i + k * colSize] = result[min_idx + k * colSize];
|
||||
result[min_idx + k * colSize] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void convertDateToJD2000(const T1 time, T2 julianDate) {
|
||||
// time = { Y, M, D, h, m,s}
|
||||
// time in sec and microsec -> The Epoch (unixtime)
|
||||
julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
|
||||
floor(275 * time[1] / 9) + time[2] +
|
||||
(60 * time[3] + time[4] + (time(5) / 60)) / 1440;
|
||||
}
|
||||
|
||||
static T1 convertUnixToJD2000(timeval time) {
|
||||
// time = {{s},{us}}
|
||||
T1 julianDate2000;
|
||||
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
|
||||
return julianDate2000;
|
||||
}
|
||||
|
||||
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
|
||||
// convention q = [qx,qy,qz, qw]
|
||||
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
|
||||
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
|
||||
|
||||
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
|
||||
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
|
||||
|
||||
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
|
||||
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
|
||||
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
|
||||
}
|
||||
|
||||
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
|
||||
T2 *cartesianOutput) {
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
* longitude and altitude
|
||||
* @param: lat geodetic latitude [rad]
|
||||
* longi longitude [rad]
|
||||
* alt altitude [m]
|
||||
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double radiusPolar = 6356752.314;
|
||||
double radiusEqua = 6378137;
|
||||
|
||||
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
|
||||
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
|
||||
|
||||
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
|
||||
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
|
||||
}
|
||||
|
||||
static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
|
||||
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
|
||||
* cartesian coordinates in ECEF
|
||||
* @param: x x-value of position vector [m]
|
||||
* y y-value of position vector [m]
|
||||
* z z-value of position vector [m]
|
||||
* latitude geodetic latitude [rad]
|
||||
* longitude longitude [rad]
|
||||
* altitude altitude [m]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
// From World Geodetic System the Earth Radii
|
||||
double a = 6378137.0; // semimajor axis [m]
|
||||
double b = 6356752.3142; // semiminor axis [m]
|
||||
|
||||
// Calculation
|
||||
double e2 = 1 - pow(b, 2) / pow(a, 2);
|
||||
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
|
||||
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
|
||||
double p = std::abs(vector[2]) / epsilon2;
|
||||
double s = pow(rho, 2) / (e2 * epsilon2);
|
||||
double q = pow(p, 2) - pow(b, 2) + s;
|
||||
double u = p / sqrt(q);
|
||||
double v = pow(b, 2) * pow(u, 2) / q;
|
||||
double P = 27 * v * s / q;
|
||||
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
|
||||
double t = (1 + Q + 1 / Q) / 6;
|
||||
double c = sqrt(pow(u, 2) - 1 + 2 * t);
|
||||
double w = (c - u) / 2;
|
||||
double d =
|
||||
sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
|
||||
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
|
||||
latitude = asin((epsilon2 + 1) * d / N);
|
||||
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
|
||||
longitude = atan2(vector[1], vector[0]);
|
||||
}
|
||||
|
||||
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* @param: time Current time
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double JD2000Floor = 0;
|
||||
double JD2000 = convertUnixToJD2000(time);
|
||||
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
|
||||
JD2000Floor = floor(JD2000);
|
||||
if ((JD2000 - JD2000Floor) < 0.5) {
|
||||
JD2000Floor -= 0.5;
|
||||
} else {
|
||||
JD2000Floor += 0.5;
|
||||
}
|
||||
|
||||
double JC2000 = JD2000Floor / 36525;
|
||||
double sec = (JD2000 - JD2000Floor) * 86400;
|
||||
double gmst = 0; // greenwich mean sidereal time
|
||||
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
|
||||
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
|
||||
double rest = gmst / 86400;
|
||||
double FloorRest = floor(rest);
|
||||
double secOfDay = rest - FloorRest;
|
||||
secOfDay *= 86400;
|
||||
gmst = secOfDay / 240 * M_PI / 180;
|
||||
|
||||
outputDcmEJ[0] = cos(gmst);
|
||||
outputDcmEJ[1] = sin(gmst);
|
||||
outputDcmEJ[2] = 0;
|
||||
outputDcmEJ[3] = -sin(gmst);
|
||||
outputDcmEJ[4] = cos(gmst);
|
||||
outputDcmEJ[5] = 0;
|
||||
outputDcmEJ[6] = 0;
|
||||
outputDcmEJ[7] = 0;
|
||||
outputDcmEJ[8] = 1;
|
||||
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
|
||||
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
|
||||
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
|
||||
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
|
||||
}
|
||||
|
||||
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* give also the back the derivative of this matrix
|
||||
* @param: unixTime Current time in Unix format
|
||||
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
|
||||
* outputDotDcmEJ Derivative of transformation matrix [3][3]
|
||||
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
|
||||
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
|
||||
* Oliver Zeile
|
||||
*
|
||||
https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
|
||||
static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
|
||||
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
|
||||
//(initial Offset) International Atomic Time (TAI)
|
||||
|
||||
double JD2000UTC1 = convertUnixToJD2000(unixTime);
|
||||
|
||||
// Julian Date / century from TT
|
||||
timeval terestrialTime = unixTime;
|
||||
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
|
||||
double JD2000TT = convertUnixToJD2000(terestrialTime);
|
||||
double JC2000TT = JD2000TT / 36525;
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth rotation Theta
|
||||
//-------------------------------------------------------------------------------------
|
||||
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// Earth Rotation angle
|
||||
double era = 0;
|
||||
era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
|
||||
// Greenwich Mean Sidereal Time
|
||||
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
|
||||
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
|
||||
double arcsecFactor = 1 * M_PI / (180 * 3600);
|
||||
gmst2000 *= arcsecFactor;
|
||||
gmst2000 += era;
|
||||
|
||||
theta[0][0] = cos(gmst2000);
|
||||
theta[0][1] = sin(gmst2000);
|
||||
theta[0][2] = 0;
|
||||
theta[1][0] = -sin(gmst2000);
|
||||
theta[1][1] = cos(gmst2000);
|
||||
theta[1][2] = 0;
|
||||
theta[2][0] = 0;
|
||||
theta[2][1] = 0;
|
||||
theta[2][2] = 1;
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Precession P
|
||||
//-------------------------------------------------------------------------------------
|
||||
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
|
||||
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
|
||||
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
|
||||
double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
|
||||
|
||||
zeta *= arcsecFactor;
|
||||
theta2 *= arcsecFactor;
|
||||
ze *= arcsecFactor;
|
||||
|
||||
precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
|
||||
precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
|
||||
precession[2][0] = sin(theta2) * cos(zeta);
|
||||
precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
|
||||
precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
|
||||
precession[2][1] = -sin(theta2) * sin(zeta);
|
||||
precession[0][2] = -cos(ze) * sin(theta2);
|
||||
precession[1][2] = -sin(ze) * sin(theta2);
|
||||
precession[2][2] = cos(theta2);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Transformation from earth Nutation N
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// lunar asc node
|
||||
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
|
||||
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
|
||||
Om *= arcsecFactor;
|
||||
// delta psi approx
|
||||
double dp = -17.2 * arcsecFactor * sin(Om);
|
||||
|
||||
// delta eps approx
|
||||
double de = 9.203 * arcsecFactor * cos(Om);
|
||||
|
||||
// % true obliquity of the ecliptic eps p.71 (simplified)
|
||||
double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
|
||||
|
||||
nutation[0][0] = cos(dp);
|
||||
nutation[1][0] = cos(e + de) * sin(dp);
|
||||
nutation[2][0] = sin(e + de) * sin(dp);
|
||||
nutation[0][1] = -cos(e) * sin(dp);
|
||||
nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
|
||||
nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
|
||||
nutation[0][2] = -sin(e) * sin(dp);
|
||||
nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
|
||||
nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of Derivative of rotation matrix from earth
|
||||
//-------------------------------------------------------------------------------------
|
||||
double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
|
||||
double omegaEarth = 0.000072921158553;
|
||||
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of transformation matrix and Derivative of transformation matrix
|
||||
//-------------------------------------------------------------------------------------
|
||||
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
|
||||
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
|
||||
}
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
|
||||
int i, j;
|
||||
double determinant = 0;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
|
||||
{matrix[3], matrix[4], matrix[5]},
|
||||
{matrix[6], matrix[7], matrix[8]}};
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
|
||||
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
|
||||
}
|
||||
// cout<<"\n\ndeterminant: "<<determinant;
|
||||
// cout<<"\n\nInverse of matrix is: \n";
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
|
||||
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
|
||||
determinant;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
|
||||
/* do not use this. takes 300ms */
|
||||
float det = 0;
|
||||
T1 matrix[size][size], submatrix[size - 1][size - 1];
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
if (size == 2)
|
||||
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
|
||||
else {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
int subRow = 0;
|
||||
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
|
||||
int subCol = 0;
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
if (colIndex == col) continue;
|
||||
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
|
||||
subCol++;
|
||||
}
|
||||
subRow++;
|
||||
}
|
||||
det += (pow(-1, col) * matrix[0][col] *
|
||||
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
|
||||
}
|
||||
}
|
||||
return det;
|
||||
}
|
||||
|
||||
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
||||
// Stopwatch stopwatch;
|
||||
T1 matrix[size][size], identity[size][size];
|
||||
// reformat array to matrix
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
// init identity matrix
|
||||
std::memset(identity, 0.0, sizeof(identity));
|
||||
for (uint8_t diag = 0; diag < size; diag++) {
|
||||
identity[diag][diag] = 1;
|
||||
}
|
||||
// gauss-jordan algo
|
||||
// sort matrix such as no diag entry shall be 0
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
bool swaped = false;
|
||||
uint8_t rowIndex = 0;
|
||||
while ((rowIndex < size) && !swaped) {
|
||||
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
swaped = true;
|
||||
}
|
||||
rowIndex++;
|
||||
}
|
||||
if (!swaped) {
|
||||
return 1; // matrix not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
uint8_t rowIndex;
|
||||
if (row == 0) {
|
||||
rowIndex = size - 1;
|
||||
} else {
|
||||
rowIndex = row - 1;
|
||||
}
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
row--;
|
||||
if (row < 0) {
|
||||
return 1; // Matrix is not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
// remove non diag elements in matrix (jordan)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
|
||||
if (row != rowIndex) {
|
||||
double ratio = matrix[rowIndex][row] / matrix[row][row];
|
||||
for (int colIndex = 0; colIndex < size; colIndex++) {
|
||||
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
|
||||
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// normalize rows in matrix (gauss)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int col = 0; col < size; col++) {
|
||||
identity[row][col] = identity[row][col] / matrix[row][row];
|
||||
}
|
||||
}
|
||||
std::memcpy(inverse, identity, sizeof(identity));
|
||||
return 0; // successful inversion
|
||||
}
|
||||
|
||||
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
|
||||
for (uint8_t i = 0; i < size; i++) {
|
||||
if (not isfinite(inputVector[i])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
|
||||
for (uint8_t col = 0; col < cols; col++) {
|
||||
for (uint8_t row = 0; row < rows; row++) {
|
||||
if (not isfinite(inputMatrix[row * cols + cols])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* ACS_MATH_MATHOPERATIONS_H_ */
|
||||
Loading…
Reference in New Issue
Block a user