From 54ba1d1a9710e079839a792d113250bfbaef0887 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 4 Mar 2020 09:09:22 +1100 Subject: [PATCH] EKF: Improve protection from bad updates --- EKF/EKFGSF_yaw.cpp | 33 +++++++++++++++++++++++++-------- EKF/EKFGSF_yaw.h | 3 ++- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 6d152f898d..83925dacec 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -59,20 +59,36 @@ void EKFGSF_yaw::update(const Vector3f del_ang, // IMU delta angle rotation vect } else { float total_w = 0.0f; float newWeight[N_MODELS_EKFGSF]; + float bad_update = false; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { // subsequent measurements are fused as direct state observations - updateEKF(model_index); + if (!updateEKF(model_index)) { + bad_update = true; + } + } - // calculate weighting for each model assuming a normal distribution - newWeight[model_index]= fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f); - total_w += newWeight[model_index]; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { + if (bad_update) { + // keep previous weight + newWeight[model_index] = _ekf_gsf[model_index].W; + } else { + // calculate weighting for each model assuming a normal distribution + newWeight[model_index] = fmaxf(gaussianDensity(model_index) * _ekf_gsf[model_index].W, 0.0f); + total_w += newWeight[model_index]; + } } // normalise the weighting function - if (_ekf_gsf_vel_fuse_started && total_w > 0.0f) { + if (_ekf_gsf_vel_fuse_started && total_w > 1e-6f) { float total_w_inv = 1.0f / total_w; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - _ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv; + _ekf_gsf[model_index].W = newWeight[model_index] * total_w_inv; + } + } else { + // assign even weights + float default_weight = 1.0f / (float)N_MODELS_EKFGSF; + for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { + _ekf_gsf[model_index].W = default_weight; } } @@ -338,7 +354,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) } // Update EKF states and covariance for specified model index using velocity measurement -void EKFGSF_yaw::updateEKF(const uint8_t model_index) +bool EKFGSF_yaw::updateEKF(const uint8_t model_index) { // set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f)); @@ -393,7 +409,7 @@ void EKFGSF_yaw::updateEKF(const uint8_t model_index) t7 = 1.0f/t6; } else { // skip this fusion step - return; + return false; } const float t8 = P11+velObsVar; const float t10 = P00+velObsVar; @@ -474,6 +490,7 @@ void EKFGSF_yaw::updateEKF(const uint8_t model_index) _ahrs_ekf_gsf[model_index].R(1,1) = R_prev[0][1] * sinYaw + R_prev[1][1] * cosYaw; _ahrs_ekf_gsf[model_index].R(1,2) = R_prev[0][2] * sinYaw + R_prev[1][2] * cosYaw; + return true; } void EKFGSF_yaw::initialiseEKFGSF() diff --git a/EKF/EKFGSF_yaw.h b/EKF/EKFGSF_yaw.h index eebb04bf32..f1de98893a 100644 --- a/EKF/EKFGSF_yaw.h +++ b/EKF/EKFGSF_yaw.h @@ -126,7 +126,8 @@ class EKFGSF_yaw void predictEKF(const uint8_t model_index); // update state and covariance for the specified EKF using a NE velocity measurement - void updateEKF(const uint8_t model_index); + // return false if update failed + bool updateEKF(const uint8_t model_index); inline float sq(float x) { return x * x; };