From a6cd04ca4cc33b5837c86a584e3ba76051bb6f15 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 7 Sep 2014 09:14:13 +1000 Subject: [PATCH] AP_NavEKF : Fix bug in GPS innovation variance growth calculation The uncertainty in acceleration is currently only scaled using horizontal accelerations, however during vertical plane aerobatics and high g pullups, misalignment in angles can cause significant horizontal acceleration error. The scaling now uses the 3D acceleration vector length to better work during vertical plane high g maneouvres. This error was found during flight testing with 8g pullups --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 8d57c0bdd1..52b15bc28f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1714,7 +1714,7 @@ void NavEKF::FuseVelPosNED() // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate max valid position innovation squared based on a maximum horizontal inertial nav accel error and GPS noise parameter // max inertial nav error is scaled with horizontal g to allow for increased errors when manoeuvring - float accelScale = (1.0f + 0.1f * accNavMagHoriz); + float accelScale = (1.0f + 0.1f * accNavMag); float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata);