From 29243ac5cbb5d27ac71744e88afcd786df6f748d Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 5 May 2021 10:36:58 +0200 Subject: [PATCH] yaw_reset: reduce minimum vector length to compute yaw error A value of 1m/s was too conservative and the EKF takes too much time to trigger a yaw reset. The value can be safely reduced because the threshold is computed using the accuracy of the measurement and estimate before being limited by this value. --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 069b098041..a34d9ea97d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -617,8 +617,8 @@ void Ekf::controlGpsFusion() const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared(); const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared(); - const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), 1.f); - const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), 1.f); + const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f)); + const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f)); if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) { const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());