Draft: feat(ekf2): airspeed fusion for wind estimation in rotary-wing flight

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -66,9 +66,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		_external_wind_init = false;
 	}
 
-#if defined(CONFIG_EKF2_GNSS)
+	const bool fixed_wing_or_transition = _control_status.flags.fixed_wing || _control_status.flags.in_transition;
 
-	// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
+#if defined(CONFIG_EKF2_GNSS)
+	// EKF-GSF airspeed compensation assumes fixed-wing dynamics.
 	if (_control_status.flags.fixed_wing) {
 		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
 			if (!_control_status.flags.fuse_aspd) {
@@ -78,6 +79,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		} else {
 			_yawEstimator.setTrueAirspeed(0.f);
 		}
+
+	} else if (!_control_status.flags.in_transition) {
+		_yawEstimator.setTrueAirspeed(NAN);
 	}
 
 #endif // CONFIG_EKF2_GNSS
@@ -95,8 +99,13 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 		updateAirspeed(airspeed_sample, _aid_src_airspeed);
 
-		const bool continuing_conditions_passing = _control_status.flags.in_air && (_control_status.flags.fixed_wing
-				|| _control_status.flags.in_transition)
+		// Use lateral specific force to check the zero-sideslip assumption in MC mode.
+		const bool mc_airflow_aligned = (_params.ekf2_aspd_mc_lim <= 0.f)
+						|| (fabsf(_aspd_mc_lat_accel_lpf.getState()) < _params.ekf2_aspd_mc_lim);
+		const bool mc_aspd_allowed = (_params.ekf2_aspd_mc == 1) && mc_airflow_aligned;
+
+		const bool continuing_conditions_passing = _control_status.flags.in_air
+				&& (fixed_wing_or_transition || mc_aspd_allowed)
 				&& !_control_status.flags.fake_pos;
 
 		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
@@ -112,7 +121,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 				}
 
 #if defined(CONFIG_EKF2_GNSS)
-				_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
+
+				if (fixed_wing_or_transition) {
+					_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
+				}
+
 #endif // CONFIG_EKF2_GNSS
 
 				const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
@@ -126,8 +139,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			}
 
 		} else if (starting_conditions_passing) {
-			const bool do_vel_reset = _horizontal_deadreckon_time_exceeded
-						  || (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent);
+			// Never reset navigation states from airspeed in MC mode.
+			const bool do_vel_reset = (_horizontal_deadreckon_time_exceeded
+						   || (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent))
+						  && fixed_wing_or_transition;
 
 			const Vector2f wind_vel_var = getWindVelocityVariance();
 			const bool do_wind_reset = (!_control_status.flags.wind || ((wind_vel_var(0) + wind_vel_var(1)) > sq(_params.initial_wind_uncertainty)))
@@ -192,8 +207,9 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		return;
 	}
 
-	// determine if we need the airspeed fusion to correct states other than wind
-	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
+	// Airspeed never corrects navigation states in MC mode.
+	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning
+				      || !(_control_status.flags.fixed_wing || _control_status.flags.in_transition);
 
 	const float innov_var = aid_src.innovation_variance;
 

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -97,8 +97,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		return false;
 	}
 
-	// determine if we need the sideslip fusion to correct states other than wind
-	bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
+	// Synthetic sideslip never corrects navigation states in MC mode.
+	const bool update_wind_only = !_control_status.flags.wind_dead_reckoning
+				      || !(_control_status.flags.fixed_wing || _control_status.flags.in_transition);
 
 	// Reset covariance and states if the calculation is badly conditioned
 	if ((sideslip.innovation_variance < sideslip.observation_variance)

src/modules/ekf2/EKF/common.h

@@ -424,6 +424,8 @@ struct parameters {
 	float ekf2_tas_gate{5.0f};              ///< True Airspeed innovation consistency gate size (STD)
 	float ekf2_eas_noise{1.4f};             ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
 	float ekf2_arsp_thr{2.0f};              ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+	int32_t ekf2_aspd_mc{0};                ///< Enable airspeed fusion for wind estimation in MC mode
+	float ekf2_aspd_mc_lim{1.0f};           ///< MC airspeed fusion lateral specific force limit (m/sec**2); 0 disables
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)

src/modules/ekf2/EKF/ekf.cpp

@@ -275,6 +275,10 @@ void Ekf::predictState(const imuSample &imu_delayed)
 
 	// calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere
 	_accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+	_aspd_mc_lat_accel_lpf.update(corrected_delta_vel(1) / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);
+#endif // CONFIG_EKF2_AIRSPEED
 }
 
 bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,

src/modules/ekf2/EKF/ekf.h

@@ -550,6 +550,11 @@ class Ekf final : public EstimatorInterface
 	static constexpr float _kAccelHorizLpfTimeConstant = 1.f;
 	AlphaFilter<Vector2f> _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 
+#if defined(CONFIG_EKF2_AIRSPEED)
+	static constexpr float _kAspdMcLatAccelLpfTimeConstant = 1.f;
+	AlphaFilter<float> _aspd_mc_lat_accel_lpf{_kAspdMcLatAccelLpfTimeConstant}; ///< Filtered body-Y specific force (m/sec**2)
+#endif // CONFIG_EKF2_AIRSPEED
+
 #if defined(CONFIG_EKF2_WIND)
 	static constexpr float _kHeightRateLpfTimeConstant = 10.f;
 	AlphaFilter<float> _height_rate_lpf{_kHeightRateLpfTimeConstant};

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -842,8 +842,9 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 
-	// air data aiding active
-	if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
+	// MC air data cannot provide dead-reckoning aiding.
+	if ((_control_status.flags.fixed_wing || _control_status.flags.in_transition)
+	    && (_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
 	    && (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
 	   ) {
 		// wind_dead_reckoning: no other aiding but air data

src/modules/ekf2/EKF2.cpp

@@ -118,6 +118,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_tas_gate(_params->ekf2_tas_gate),
 	_param_ekf2_eas_noise(_params->ekf2_eas_noise),
 	_param_ekf2_arsp_thr(_params->ekf2_arsp_thr),
+	_param_ekf2_aspd_mc(_params->ekf2_aspd_mc),
+	_param_ekf2_aspd_mc_lim(_params->ekf2_aspd_mc_lim),
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_SIDESLIP)
 	_param_ekf2_beta_gate(_params->ekf2_beta_gate),

src/modules/ekf2/EKF2.hpp

@@ -610,6 +610,10 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		// control of airspeed fusion
 		(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
+		(ParamExtInt<px4::params::EKF2_ASPD_MC>)
+		_param_ekf2_aspd_mc, ///< Airspeed fusion for wind estimation in MC mode
+		(ParamExtFloat<px4::params::EKF2_ASPD_MC_LIM>)
+		_param_ekf2_aspd_mc_lim, ///< MC airspeed fusion lateral specific force limit (m/s^2)
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_SIDESLIP)

src/modules/ekf2/params_airspeed.yaml

@@ -8,12 +8,33 @@ parameters:
         long: Airspeed data is fused for wind estimation if above this threshold.
           Set to 0 to disable airspeed fusion. For reliable wind estimation both sideslip
           (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. Only applies
-          to fixed-wing vehicles (or VTOLs in fixed-wing mode).
+          to fixed-wing vehicles (or VTOLs in fixed-wing mode), unless EKF2_ASPD_MC
+          is enabled.
       type: float
       default: 0.0
       min: 0.0
       unit: m/s
       decimal: 1
+    EKF2_ASPD_MC:
+      description:
+        short: Airspeed fusion in multicopter mode
+        long: Allow true airspeed fusion for wind estimation in multicopter mode.
+          Only the wind states are corrected. Fusion stops when lateral specific force
+          exceeds EKF2_ASPD_MC_LIM.
+      type: boolean
+      default: 0
+    EKF2_ASPD_MC_LIM:
+      description:
+        short: Multicopter lateral specific force limit
+        long: Maximum low-pass filtered body-Y specific force for airspeed fusion in
+          multicopter mode. Fusion stops above this value because the airspeed and
+          sideslip models assume zero sideslip. Set to 0 to disable the check.
+      type: float
+      default: 1.0
+      min: 0.0
+      max: 5.0
+      unit: m/s^2
+      decimal: 2
     EKF2_ASP_DELAY:
       description:
         short: Airspeed measurement delay relative to IMU measurements

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

@@ -51,7 +51,7 @@ class Airspeed: public Sensor
 	Airspeed(std::shared_ptr<Ekf> ekf);
 	~Airspeed();
 
-	void setData(float true_airspeed, float eas2tas);
+	void setData(float true_airspeed, float indicated_airspeed);
 
 private:
 	float _true_airspeed_data{0.0f};

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -75,6 +75,32 @@ class EkfAirspeedTest : public ::testing::Test
 	void TearDown() override
 	{
 	}
+
+	void startExternalVisionVelocity(const Vector3f &velocity)
+	{
+		_ekf_wrapper.enableExternalVisionVelocityFusion();
+		_sensor_simulator._vio.setVelocity(velocity);
+		_sensor_simulator._vio.setVelocityFrameToLocalNED();
+		_sensor_simulator.startExternalVision();
+
+		// Allow EV fusion to reset velocity.
+		_sensor_simulator.runSeconds(0.5);
+	}
+
+	void startMulticopterAirspeedFusionScenario(bool mc_airspeed_enabled, float true_airspeed)
+	{
+		parameters *params = _ekf->getParamHandle();
+		params->ekf2_arsp_thr = 2.0f;
+		params->ekf2_aspd_mc = mc_airspeed_enabled ? 1 : 0;
+		params->ekf2_aspd_mc_lim = 1.0f;
+		_ekf_wrapper.enableBetaFusion();
+
+		_ekf->set_in_air_status(true);
+		_ekf->set_vehicle_at_rest(false);
+		_ekf->set_is_fixed_wing(false);
+		_sensor_simulator.startAirspeedSensor();
+		_sensor_simulator._airspeed.setData(true_airspeed, true_airspeed);
+	}
 };
 
 TEST_F(EkfAirspeedTest, testWindVelocityEstimation)
@@ -346,3 +372,79 @@ TEST_F(EkfAirspeedTest, testExternalWindResetOnGround)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
 	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 }
+
+TEST_F(EkfAirspeedTest, testMulticopterAirspeedFusionRequiresParam)
+{
+	startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f));
+	startMulticopterAirspeedFusionScenario(false, 2.4f);
+
+	_sensor_simulator.runSeconds(2);
+
+	EXPECT_FALSE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_FALSE(_ekf_wrapper.isWindVelocityEstimated());
+}
+
+TEST_F(EkfAirspeedTest, testMulticopterWindVelocityEstimation)
+{
+	const Vector3f simulated_velocity_earth(0.0f, 1.5f, 0.0f);
+	const Vector2f airspeed_body(2.4f, 0.0f);
+	startExternalVisionVelocity(simulated_velocity_earth);
+	startMulticopterAirspeedFusionScenario(true, airspeed_body(0));
+
+	_sensor_simulator.runSeconds(15);
+
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+
+	const Dcmf R_to_earth_sim(_quat_sim);
+	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
+	const Vector3f vel_wind_expected = simulated_velocity_earth - R_to_earth_sim * Vector3f(airspeed_body(0),
+					   airspeed_body(1), 0.0f);
+
+	EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1e-1f);
+	EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1e-1f);
+}
+
+TEST_F(EkfAirspeedTest, testMulticopterAirspeedFusionStopsWhenMisaligned)
+{
+	startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f));
+	startMulticopterAirspeedFusionScenario(true, 2.4f);
+
+	_sensor_simulator.runSeconds(2);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+
+	// Exceed the lateral specific force limit.
+	_sensor_simulator._imu.setAccelData(Vector3f(0.0f, 3.0f, -CONSTANTS_ONE_G));
+	_sensor_simulator.runSeconds(2);
+
+	EXPECT_FALSE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingBetaFusion());
+
+	// Fusion restarts once the specific force drops below the limit.
+	_sensor_simulator._imu.setAccelData(Vector3f(0.0f, 0.0f, -CONSTANTS_ONE_G));
+	_sensor_simulator.runSeconds(5);
+
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+}
+
+TEST_F(EkfAirspeedTest, testMulticopterAirspeedNoDeadReckoning)
+{
+	startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f));
+	startMulticopterAirspeedFusionScenario(true, 8.0f);
+
+	_sensor_simulator.runSeconds(5);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+
+	// MC air data must not count as dead-reckoning aiding.
+	_sensor_simulator.stopExternalVision();
+	_sensor_simulator.runSeconds(3);
+
+	EXPECT_FALSE(_ekf->control_status_flags().wind_dead_reckoning);
+	EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
+
+	_sensor_simulator.runSeconds(5);
+	EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
+}