Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

fix(ekf2): allow external position reset heading correction

Browse Source 
This fixes the same class of bug as #27232. The external position
reset-by-fusion path explicitly intends to strongly correct correlated
states, including heading, as documented by the existing comment next to
the artificial Kalman gain increase.

However, when VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE is handled in
EKF2::Run(), it runs before _ekf.update(), so heading_observable can
still reflect the previous controlFusionModes() result. If it is false,
clearInhibitedStateKalmanGains() inhibits the intended heading
correction.

Set heading_observable before the external position fusion so the reset
can preserve the documented correction of correlated states. The normal
EKF update path recomputes the flag afterwards.
This commit is contained in:
Wang Jiayue
2026-05-08 13:26:31 +08:00
committed by Mathieu Bresciani
parent 2644109e2e
commit 456faf6881
1 changed files with 5 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+5
View File
@@ -356,6 +356,11 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
} else {
ECL_INFO("fuse external observation as position measurement");
// External position reset by fusion can correct heading through cross-correlations.
// Setting heading_observable = true prevents clearInhibitedStateKalmanGains() from
// inhibiting the update.
_control_status.flags.heading_observable = true;
VectorState H;
VectorState K;
Vector2f innov_var_temp = Vector2f(getStateVariance<State::pos>()) + 0.1f;