From 759d217d5a64984869c98f55de7ef816ea61c94c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 5 Oct 2020 21:30:51 +1100 Subject: [PATCH] EKF: Improve protection for GSF weight collapse --- EKF/EKFGSF_yaw.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index b08f977a62..7782037644 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -79,16 +79,22 @@ void EKFGSF_yaw::update(const imuSample& imu_sample, if (!bad_update) { float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution + const float min_weight = 1E-5f; + uint8_t n_weight_clips = 0; 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), 1E-5f); + _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index); + if (_model_weights(model_index) < min_weight) { + n_weight_clips++; + _model_weights(model_index) = min_weight; + } total_weight += _model_weights(model_index); } // normalise the weighting function - if (total_weight > 1e-10f) { + if (n_weight_clips < N_MODELS_EKFGSF) { _model_weights /= total_weight; } else { - // calculation has collapsed so reset + // all weights have collapsed due to excessive innovation variances so reset filters initialiseEKFGSF(); }