EKF2: Improve Manual Position Reset Handling (#25885)

* 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

Author: haumarco

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<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,24 +344,38 @@ 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 {
 			ECL_INFO("fuse external observation as position measurement");
 
 			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

@@ -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));
 }