Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Fix variance constraint bug 

Constraining variances to a minimum value of 1e-9 was causing problems
with gyro bias and angular accuracy in noisy GPS environments.
Because the constraint is applied after every covariance prediction
and correction, a lower value of 0 is more appropriate.
This commit is contained in:
Paul Riseborough 2014-02-27 06:21:09 +11:00 committed by Andrew Tridgell
parent 8b59e564ba
commit d83b382e59
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2410,13 +2410,13 @@ void NavEKF::CopyAndFixCovariances()
void NavEKF::ConstrainVariances() void NavEKF::ConstrainVariances()
{ {
// Constrain variances to be within set limits // Constrain variances to be within set limits
for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,1.0f); for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f);
for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,1.0e3f); for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f);
for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,1.0e6f); for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,sq(0.175f * dtIMU)); for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMU));
P[13][13] = constrain_float(P[13][13],1.0e-9f,sq(10.0f * dtIMU)); P[13][13] = constrain_float(P[13][13],0.0f,sq(10.0f * dtIMU));
for (uint8_t i=14; i<=15; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,1.0e3f); for (uint8_t i=14; i<=15; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f);
for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,1.0f); for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f);
} }
void NavEKF::ConstrainStates() void NavEKF::ConstrainStates()