diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 06b50c4c3d..3e609eb8e4 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -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()) + 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()) + 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 } diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 6f0cdb84d1..20d323160d 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -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)); }