fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 19:18:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(ekf2): preserve EV position bias on reset when GNSS inactive

Browse Source 
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().
This commit is contained in:
Jacob Dahl
2026-03-18 00:18:14 -08:00
parent 0ac48b663c
commit 098814663b
1 changed files with 2 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
+2 -4
View File
@@ -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);
}
}