From ffde7f815c7f097d791ac3651df40d4ab930f732 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 27 Sep 2023 09:01:58 +1000 Subject: [PATCH] AP_NavEKF3: Allow wind to relearn rapidly when GPS is re-enabled --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 428130028f..435ba04dcb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1086,10 +1086,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) if (!inhibitWindStates) { const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; - treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable; - if (treatWindStatesAsTruth) { + const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable; + if (newTreatWindStatesAsTruth) { + treatWindStatesAsTruth = true; P[23][23] = P[22][22] = 0.0f; } else { + if (treatWindStatesAsTruth) { + treatWindStatesAsTruth = false; + if (windStateIsObservable) { + // allow EKF to relearn wind states rapidly + P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX); + } + } 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