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

EKF2: Improve Manual Position Reset Handling (#25885)

Browse Source 
* reset by fusion:
* state correction with tiny observation variance
* covariance matrix upate with correct observation variance

* reset wind to 0 on hard-reset during global-position-reset
increase gate

* adjust unittest: velocity gets now reset on resetGlobalPosToExternalObservation
This commit is contained in:
Marco Hauswirth
2025-12-18 15:21:09 +01:00
committed by GitHub
parent 3e425210e0
commit bbf32a537e
2 changed files with 24 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+22 -8
View File
@@ -330,7 +330,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
const Vector2f innov = (_gpos - gpos_corrected).xy();
const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
const float sq_gate = sq(5.f); // magic hardcoded gate
const float sq_gate = sq(10.f); // magic hardcoded gate
const float test_ratio = sq(innov(0)) / (sq_gate * innov_var(0)) + sq(innov(1)) / (sq_gate * innov_var(1));
const bool innov_rejected = (test_ratio > 1.f);
@@ -344,6 +344,14 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
_information_events.flags.reset_pos_to_ext_obs = true;
resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var);
Vector2f new_vel = _state.vel.xy() - _state.wind_vel;
resetHorizontalVelocityTo(new_vel, Vector2f(P(State::vel.idx, State::vel.idx), P(State::vel.idx + 1, State::vel.idx + 1)));
// bump up wind uncertainty to ensure velocity remains correct
// eventual reset-by-fusion will have larger effect on wind state
P.uncorrelateCovarianceSetVariance<2>(State::wind_vel.idx, 10.f);
_state.wind_vel.setZero();
_last_known_gpos.setLatLon(gpos_corrected);
} else {
@@ -351,17 +359,23 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
VectorState H;
VectorState K;
Vector2f innov_var_temp = Vector2f(getStateVariance<State::pos>()) + 0.1f;
for (unsigned index = 0; index < 2; index++) {
K = VectorState(P.row(State::pos.idx + index)) / innov_var(index);
// Artificially setting the observation variance to a small value to
// increase the Kalman gain to basically 1 to force a reset of the position
// through fusion. This also enforces a strong correction of the correlated states
// (e.g.: velocity, heading, wind)
// The update of the covariance matrix is still performed with the correct observation variance
// to not artificially reduce the state uncertainty and cross-correlations.
K = VectorState(P.row(State::pos.idx + index)) / innov_var_temp(index);
H(State::pos.idx + index) = 1.f;
// Artificially set the position Kalman gain to 1 in order to force a reset
// of the position through fusion. This allows the EKF to use part of the information
// to continue learning the correlated states (e.g.: velocity, heading, wind) while
// performing a position reset.
K(State::pos.idx + index) = 1.f;
measurementUpdate(K, H, obs_var, innov(index));
clearInhibitedStateKalmanGains(K);
fuse(K, innov(index));
K = VectorState(P.row(State::pos.idx + index)) / innov_var(index);
measurementUpdate(K, H, obs_var, 0.f);
H(State::pos.idx + index) = 0.f; // Reset the whole vector to 0
}
src/modules/ekf2/test/test_EKF_airspeed.cpp
+2 -2
View File
@@ -280,7 +280,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1));
// AND WHEN: only the lat/lon is valid
@@ -293,7 +293,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
}