From 5d70854c08b228bb80ce2c215037e7acfe084adc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 15 Apr 2015 17:18:31 +1000 Subject: [PATCH] AP_NavEKF: Fix minor bug in calculation of innovation variance the innovation variance for GPS should be the sum of squares of the state and measurement uncertainty. --- 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 d1c49b81f8..af00aa826b 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1988,7 +1988,7 @@ void NavEKF::FuseVelPosNED() // 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 * accNavMag); - float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - lastPosPassTime))); + float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise) + sq(0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(imuSampleTime_ms - lastPosPassTime))); posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); // declare a timeout condition if we have been too long without data or not aiding