diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 3fed05741d6..5b627322dd1 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2048,7 +2048,12 @@ void NavEKF::FuseVelPosNED() // calculate weighting used by fuseVelPosNED to do IMU accel data blending // this is used to detect and compensate for aliasing errors with the accelerometers // provide for a first order lowpass filter to reduce noise on the weighting if required - IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting; // filter currently inactive + // set weighting to 0.5 when on ground to allow more rapid learning of bias errors without 'ringing' in bias estimates + if (vehicleArmed) { + IMU1_weighting = 1.0f * (K1 / (K1 + K2)) + 0.0f * IMU1_weighting; // filter currently inactive + } else { + IMU1_weighting = 0.5f; + } // apply an innovation consistency threshold test, but don't fail if bad IMU data // calculate the test ratio velTestRatio = innovVelSumSq / (varVelSum * sq(_gpsVelInnovGate));