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