ekf2: allow wind dead-reckoning after manual position reset

Reset velocity using airspeed and start navigating

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -93,7 +93,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
-		                                         && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion
+		                                         && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);
 
 		if (_control_status.flags.fuse_aspd) {
 			if (continuing_conditions_passing) {
@@ -118,17 +118,16 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		} else if (starting_conditions_passing) {
 			ECL_INFO("starting airspeed fusion");
 
-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
-			const Vector2f wind_var_xy = getWindVelocityVariance();
+			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
+				resetVelUsingAirspeed(airspeed_sample);
 
-			if (!_control_status.flags.wind || (wind_var_xy(0) + wind_var_xy(1) > sq(_params.initial_wind_uncertainty))) {
-				// activate the wind states
-				_control_status.flags.wind = true;
-				// reset the wind speed states and corresponding covariances
+			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
+				// If starting wind state estimation, reset the wind states and covariances before fusing any data
+				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
 				resetWindUsingAirspeed(airspeed_sample);
 			}
 
+			_control_status.flags.wind = true;
 			_control_status.flags.fuse_aspd = true;
 		}
 
@@ -247,3 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 
 	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
 }
+
+void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample)
+{
+	const float euler_yaw = getEulerYaw(_R_to_earth);
+
+	// Estimate velocity using zero sideslip assumption and airspeed measurement
+	Vector2f horizontal_velocity;
+	horizontal_velocity(0) = _state.wind_vel(0) + airspeed_sample.true_airspeed * cosf(euler_yaw);
+	horizontal_velocity(1) = _state.wind_vel(1) + airspeed_sample.true_airspeed * sinf(euler_yaw);
+
+	float vel_var = NAN; // Do not reset the velocity variance as wind variance estimate is most likely not correct
+	resetHorizontalVelocityTo(horizontal_velocity, vel_var);
+
+	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
+}

src/modules/ekf2/EKF/ekf.h

@@ -788,6 +788,7 @@ class Ekf final : public EstimatorInterface
 
 	// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
 	void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
+	void resetVelUsingAirspeed(const airspeedSample &airspeed_sample);
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -100,6 +100,11 @@ bool EkfWrapper::isIntendingBetaFusion() const
 	return _ekf->control_status_flags().fuse_beta;
 }
 
+bool EkfWrapper::isIntendingAirspeedFusion() const
+{
+	return _ekf->control_status_flags().fuse_aspd;
+}
+
 void EkfWrapper::enableGpsFusion()
 {
 	_ekf_params->gnss_ctrl |= static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL);

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -73,6 +73,8 @@ class EkfWrapper
 	void disableBetaFusion();
 	bool isIntendingBetaFusion() const;
 
+	bool isIntendingAirspeedFusion() const;
+
 	void enableGpsFusion();
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -166,3 +166,39 @@ TEST_F(EkfAirspeedTest, testResetWindUsingAirspeed)
 	EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1.f);
 	EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1.f);
 }
+
+TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
+{
+	const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f);
+	const Vector2f airspeed_body(15.f, 0.0f);
+	_sensor_simulator.runSeconds(10);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	const double latitude_new  = -15.0000005;
+	const double longitude_new = -115.0000005;
+	const float altitude_new  = 1500.0;
+
+	_ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new);
+	_ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, 50.f, 0);
+
+	// Simulate the fact that the sideslip can start immediately, without
+	// waiting for a measurement sample.
+	_ekf_wrapper.enableBetaFusion();
+	_sensor_simulator.runSeconds(1.f);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0));
+	_sensor_simulator.runSeconds(1.f);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+	const Vector3f vel = _ekf->getVelocity();
+	EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f);
+	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
+	EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
+	EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);
+}