AP_NavEKF: Added gyro bias variance scale for on-ground

11 years ago · aed6c79135
1 changed files with 1 additions and 0 deletions
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@ -374,6 +374,7 @@ void NavEKF::CovariancePrediction()
				@@ -374,6 +374,7 @@ void NavEKF::CovariancePrediction()

    for (uint8_t i= 0; i<=9;  i++) processNoise[i] = 1.0e-9f;
    for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
+    if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale;
    for (uint8_t i=13; i<=15; i++) processNoise[i] = dVelBiasSigma;
    for (uint8_t i=16; i<=17; i++) processNoise[i] = windVelSigma;
    for (uint8_t i=18; i<=20; i++) processNoise[i] = magEarthSigma;