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).
2023-06-05 16:16:02 +02:00
1 changed files with 1 additions and 2 deletions
--- 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);