diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 24018ad9..7fe33814 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -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 { diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 5f7facd2..3d97c880 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -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}};