From 3e4e4b8561069bccbe7055b73aa6d28b48e7a72f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 5 Jun 2023 16:10:14 +0200 Subject: [PATCH] ekf2_ev: do not reset learned bias When ev resets without GNSS currently active, still consider the position bias that was learned so far (GNSS fusion was maybe used in the past and a meaningful bias was esimated). --- src/modules/ekf2/EKF/ev_pos_control.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index 980a488672..fe2513f319 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -230,8 +230,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (!_control_status.flags.gps) { 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(-Vector2f(_state.pos.xy()) + measurement);