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);