From a2a50938815026ac26ef25136f3aac75cbbc979c Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 30 Aug 2022 16:37:12 +0200 Subject: [PATCH] ekf2: relax zero velocity update The ZVU is too strong and prevents the EKF from following variations in the height aiding sources, creating large innovations. --- src/modules/ekf2/EKF/zero_velocity_update.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp index a1a7530f09..ebfeb17f4e 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp @@ -51,7 +51,9 @@ void Ekf::controlZeroVelocityUpdate() Vector3f vel_obs{0, 0, 0}; Vector3f innovation = _state.vel - vel_obs; - const float obs_var = sq(0.001f); + // Set a low variance initially for faster accel bias learning and higher + // later to let the states follow the measurements + const float obs_var = _NED_origin_initialised ? sq(0.2f) : sq(0.001f); Vector3f innov_var{ P(4, 4) + obs_var, P(5, 5) + obs_var,