eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 3 Releases 117 Wiki Activity

acsParameters in kalman filter added
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt 2022-12-05 21:31:52 +01:00
parent 8d1cbd9f8b
commit 2b445369fd
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
mission/controller/acs/AcsParameters.h
View File

@ -872,8 +872,8 @@ class AcsParameters /*: public HasParametersIF*/ {
struct SunModelParameters {
float domega = 36000.771;
float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee
float m_0 = 357.5256; // coefficients for mean anomaly
float omega_0 = 280.46 * M_PI / 180.; //282.94 * M_PI / 180.; // RAAN plus argument of perigee
float m_0 = 357.5277; //357.5256; // 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.;
@ -886,10 +886,10 @@ class AcsParameters /*: public HasParametersIF*/ {
double sensorNoiseSTR = 0.1 * M_PI / 180;
double sensorNoiseSS = 8 * M_PI / 180;
double sensorNoiseMAG = 4 * M_PI / 180;
double sensorNoiseRMU[3];
double sensorNoiseGYR = 0.1 * M_PI / 180;
double sensorNoiseArwRmu; // Angular Random Walk
double sensorNoiseBsRMU; // Bias Stability
double sensorNoiseArwGYR = 3*0.0043*M_PI / sqrt(10) / 180; // Angular Random Walk
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
} kalmanFilterParameters;
struct MagnetorquesParameter {

8
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -51,7 +51,7 @@ void MultiplicativeKalmanFilter::init(const double *magneticField_, const bool *
sigmaSun = kalmanFilterParameters->sensorNoiseSS;
sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
sigmaGyro = 0.1*3.141/180; // acs parameters
sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
double normMagB[3] = {0,0,0}, normSunB[3] = {0,0,0},
normMagJ[3] = {0,0,0}, normSunJ[3] = {0,0,0};
@ -980,10 +980,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
/* ----------- PROPAGATION ----------*/
//double sigmaU = kalmanFilterParameters->sensorNoiseBsRMU;
//double sigmaV = kalmanFilterParameters->sensorNoiseArwRmu;
double sigmaU = 3*3.141/180/3600;
double sigmaV = 3*0.0043*3.141/sqrt(10)/180;
double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
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}};