From 098814663b28af03ce0b1b8c297d2ca2d11e53a5 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 18 Mar 2026 00:18:14 -0800 Subject: [PATCH] fix(ekf2): preserve EV position bias on reset when GNSS inactive When EV position resets without GNSS active, the learned bias was being discarded and the position reset directly to the raw EV measurement. This caused a position jump to the potentially-drifted EV origin, which in HOLD mode would make the drone fly back to what it thinks is the correct waypoint. Preserve the estimated bias through the reset so the position remains consistent with the current estimate. Fixes both the EV reset path and the fusion-failing recovery path in updateEvPosFusion(). --- .../ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 4720824e32..3d767d52ca 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -243,8 +243,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (!_control_status.flags.gnss_pos) { ECL_INFO("reset to %s", EV_AID_SRC_NAME); _information_events.flags.reset_pos_to_vision = true; - resetHorizontalPositionTo(measurement, measurement_var); - _ev_pos_b_est.reset(); + resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var); } else { _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); @@ -287,8 +286,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } else { - resetHorizontalPositionTo(measurement, measurement_var); - _ev_pos_b_est.reset(); + resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var); } }