From 61b276995b366564892de4f945411afaf638841d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 26 Jun 2022 08:47:08 +1000 Subject: [PATCH] AP_NavEKF3: Lock in wind state estimates when using srag to dead reckon --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 239437ee55..65b9d02fda 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1117,12 +1117,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) lastInhibitMagStates = inhibitMagStates; if (!inhibitWindStates) { - ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); - if (!tasDataDelayed.allowFusion) { - // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor - windVelVar *= 10.0f; + const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; + if (isDragFusionDeadReckoning) { + // when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate + P[23][23] = P[22][22] = 0.0f; + } else { + ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); + if (!tasDataDelayed.allowFusion) { + // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor + windVelVar *= 10.0f; + } + for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; } - for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; } // set variables used to calculate covariance growth