From 3ad4b5731571bfecee4fd18d3e7199653297bb8f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 4 Nov 2024 15:59:03 +0100 Subject: [PATCH] ekf-yaw-est: do not reset when all weights collapse The likelyhood can be really small when the innovation is suddenly large. This can occur during a GNSS velocity glitch and shouldn't reset the filter as it would bring it back to a really vulnerable state. --- src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 7aad6620ce..865c315ece 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -132,13 +132,11 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, 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) = gaussianDensity(model_index) * _model_weights(model_index); if (_model_weights(model_index) < min_weight) { - n_weight_clips++; _model_weights(model_index) = min_weight; } @@ -146,13 +144,7 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, } // normalise the weighting function - if (n_weight_clips < N_MODELS_EKFGSF) { - _model_weights /= total_weight; - - } else { - // all weights have collapsed due to excessive innovation variances so reset filters - reset(); - } + _model_weights /= total_weight; } // Calculate a composite yaw vector as a weighted average of the states for each model. @@ -368,6 +360,7 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co const float oldYaw = _ekf_gsf[model_index].X(2); _ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor; + _ekf_gsf[model_index].innov *= innov_comp_scale_factor; const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw;