From b1ca0495e280b91cb2e79efd9a3d5cd53431f43f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jan 2025 12:50:10 -0500 Subject: [PATCH] ekf2: yaw estimator additional validity checks --- src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp | 6 +++++- src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 4 ++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 889af97a08..6ce0cebc66 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const return false; } - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); + const float yaw_var = _yawEstimator.getYawVar(); + + return (yaw_var > 0.f) + && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) + && PX4_ISFINITE(yaw_var); } bool Ekf::isYawFailure() const diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 2b2293904c..89ce999146 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); } + + if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { + reset(); + } } }