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);