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

ekf2: always decorrelate pos and vel in vel reset

Browse Source
This commit is contained in:
bresch
2025-03-31 09:39:30 +02:00
committed by Mathieu Bresciani
parent f9b811b0c5
commit 54f2652329
1 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/velocity_fusion.cpp
+14 -4
View File
@@ -81,16 +81,26 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel); const Vector2f delta_horz_vel = new_horz_vel - Vector2f(_state.vel);
_state.vel.xy() = new_horz_vel; _state.vel.xy() = new_horz_vel;
Vector2f hor_vel_var;
if (PX4_ISFINITE(new_horz_vel_var(0))) { if (PX4_ISFINITE(new_horz_vel_var(0))) {
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); hor_vel_var(0) = math::max(sq(0.01f), new_horz_vel_var(0));
} else {
hor_vel_var(0) = P(State::vel.idx, State::vel.idx);
} }
if (PX4_ISFINITE(new_horz_vel_var(1))) { if (PX4_ISFINITE(new_horz_vel_var(1))) {
P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); hor_vel_var(1) = math::max(sq(0.01f), new_horz_vel_var(1));
} else {
hor_vel_var(1) = P(State::vel.idx + 1, State::vel.idx + 1);
} }
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, P(State::pos.idx, State::pos.idx)); P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, P(State::pos.idx + 1, State::pos.idx + 1));
// Position decorrelation is also required to avoid issues when no position aiding is active
P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, getPositionVariance().xy());
_output_predictor.resetHorizontalVelocityTo(delta_horz_vel); _output_predictor.resetHorizontalVelocityTo(delta_horz_vel);