From 7b04ea0dca7aaf88f62259366b5facddfb63f2a2 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Nov 2024 11:31:20 +0100 Subject: [PATCH] ekf-yaw-est: wrap_pi when updating yaw --- src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 865c315ece..2382167a0f 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -355,14 +355,14 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co // If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma // This protects from large measurement spikes const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; - - // Correct the state vector and capture the change in yaw angle - 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; + // Correct the state vector + const Vector3f delta_state = -K * _ekf_gsf[model_index].innov; + const float yawDelta = delta_state(2); + + _ekf_gsf[model_index].X.xy() += delta_state.xy(); + _ekf_gsf[model_index].X(2) = wrap_pi(_ekf_gsf[model_index].X(2) + yawDelta); // apply the change in yaw angle to the AHRS // take advantage of sparseness in the yaw rotation matrix