From bf0d70fc90726e0391b6ecbb22b2e1b6b8e1da61 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 4 Oct 2020 10:00:41 +1100 Subject: [PATCH] EKF: Protect against collapse of GSF weights --- EKF/EKFGSF_yaw.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index ae50db5eac..b08f977a62 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -80,13 +80,16 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - _model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 0.0f); + _model_weights(model_index) = fmaxf(gaussianDensity(model_index) * _model_weights(model_index), 1E-5f); total_weight += _model_weights(model_index); } // normalise the weighting function - if (total_weight > 1e-15f) { + if (total_weight > 1e-10f) { _model_weights /= total_weight; + } else { + // calculation has collapsed so reset + initialiseEKFGSF(); } // Enforce a minimum weighting value. This was added during initial development but has not been needed