From fc65e04ef21182d038f18456819de1ba04438fa6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Jul 2019 07:50:39 +1000 Subject: [PATCH] AP_NavEKF3: fixed baro innovation gate when on ground with AIDING_NONE when on the ground without a position source we would disable the innovation gate for the barometer. This meant that a single (or small number of) really bad baro readings would be fused into the EKF, causing it to destabilise Fixes #11903 --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 93524000e7..c8fe907651 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -564,10 +564,17 @@ void NavEKF3_core::FuseVelPosNED() varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); + + // when on ground we accept a larger test ratio to allow + // the filter to handle large switch on IMU bias errors + // without rejecting the height sensor + const float maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0; + // fail if the ratio is > 1, but don't fail if bad IMU data - hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); + hgtHealth = ((hgtTestRatio < maxTestRatio) || badIMUdata); + // Fuse height data if healthy or timed out or in constant position mode - if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { + if (hgtHealth || hgtTimeout) { // Calculate a filtered value to be used by pre-flight health checks // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution if (onGround) {