From d83b382e59fe86f701bebdf706770cc05a3384d4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 27 Feb 2014 06:21:09 +1100 Subject: [PATCH] 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. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 0d9e92aa54..198c289b30 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2410,13 +2410,13 @@ void NavEKF::CopyAndFixCovariances() void NavEKF::ConstrainVariances() { // 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=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,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=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],1.0e-9f,sq(0.175f * dtIMU)); - P[13][13] = constrain_float(P[13][13],1.0e-9f,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=16; i<=21; 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],0.0f,1.0e3f); + 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],0.0f,sq(0.175f * 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],0.0f,1.0e3f); + for (uint8_t i=16; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); } void NavEKF::ConstrainStates()