ekf2: add kconfig option to enable/disable airspeed and sideslip fusion

src/modules/ekf2/EKF/CMakeLists.txt

@@ -33,7 +33,6 @@
 
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
-	airspeed_fusion.cpp
 	auxvel_fusion.cpp
 	baro_height_control.cpp
 	bias_estimator.cpp
@@ -64,13 +63,16 @@ list(APPEND EKF_SRCS
 	range_finder_consistency_check.cpp
 	range_height_control.cpp
 	sensor_range_finder.cpp
-	sideslip_fusion.cpp
 	terrain_estimator.cpp
 	vel_pos_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_velocity_update.cpp
 )
 
+if(CONFIG_EKF2_AIRSPEED)
+	list(APPEND EKF_SRCS airspeed_fusion.cpp)
+endif()
+
 if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS drag_fusion.cpp)
 endif()
@@ -79,6 +81,10 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_SIDESLIP)
+	list(APPEND EKF_SRCS sideslip_fusion.cpp)
+endif()
+
 add_library(ecl_EKF
 	${EKF_SRCS}
 )

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -222,21 +222,6 @@ void Ekf::stopAirspeedFusion()
 	}
 }
 
-float Ekf::getTrueAirspeed() const
-{
-	return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
-}
-
-void Ekf::resetWind()
-{
-	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
-		resetWindUsingAirspeed(_airspeed_sample_delayed);
-
-	} else {
-		resetWindToZero();
-	}
-}
-
 void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 {
 	const float euler_yaw = getEulerYaw(_R_to_earth);
@@ -282,14 +267,3 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
 	P(22, 22) += P(4, 4);
 	P(23, 23) += P(5, 5);
 }
-
-void Ekf::resetWindToZero()
-{
-	ECL_INFO("reset wind to zero");
-
-	// If we don't have an airspeed measurement, then assume the wind is zero
-	_state.wind_vel.setZero();
-
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
-}

src/modules/ekf2/EKF/common.h

@@ -307,7 +307,6 @@ struct parameters {
 	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
 	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
 	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
-	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
 	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float ev_delay_ms{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
@@ -364,16 +363,21 @@ struct parameters {
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	// airspeed fusion
+	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
 	float tas_innov_gate{5.0f};             ///< True Airspeed innovation consistency gate size (STD)
 	float eas_noise{1.4f};                  ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+#endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_SIDESLIP)
 	// synthetic sideslip fusion
 	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
+#endif // CONFIG_EKF2_SIDESLIP
 
 	// range finder fusion
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)

src/modules/ekf2/EKF/control.cpp

@@ -106,8 +106,14 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	controlMagFusion();
 	controlOpticalFlowFusion(imu_delayed);
 	controlGpsFusion(imu_delayed);
+
+#if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
 	controlBetaFusion(imu_delayed);
+#endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	controlDragFusion();

src/modules/ekf2/EKF/ekf.cpp

@@ -116,8 +116,12 @@ void Ekf::reset()
 
 	resetEstimatorAidStatus(_aid_src_baro_hgt);
 	resetEstimatorAidStatus(_aid_src_rng_hgt);
+#if defined(CONFIG_EKF2_AIRSPEED)
 	resetEstimatorAidStatus(_aid_src_airspeed);
+#endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_SIDESLIP)
 	resetEstimatorAidStatus(_aid_src_sideslip);
+#endif // CONFIG_EKF2_SIDESLIP
 
 	resetEstimatorAidStatus(_aid_src_fake_pos);
 	resetEstimatorAidStatus(_aid_src_fake_hgt);

src/modules/ekf2/EKF/ekf.h

@@ -204,13 +204,17 @@ public:
 	void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
 #endif // CONFIG_EKF2_DRAG_FUSION
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
 	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
 	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
+#endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_SIDESLIP)
 	void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
 	void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
 	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
+#endif // CONFIG_EKF2_SIDESLIP
 
 	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
 	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
@@ -233,9 +237,6 @@ public:
 	// get the wind velocity var
 	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }
 
-	// get the true airspeed in m/s
-	float getTrueAirspeed() const;
-
 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
 
@@ -444,8 +445,13 @@ public:
 
 	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
 	const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
+#endif // CONFIG_EKF2_SIDESLIP
 
 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
@@ -589,8 +595,12 @@ private:
 
 	estimator_aid_source1d_s _aid_src_baro_hgt{};
 	estimator_aid_source1d_s _aid_src_rng_hgt{};
+#if defined(CONFIG_EKF2_AIRSPEED)
 	estimator_aid_source1d_s _aid_src_airspeed{};
+#endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_SIDESLIP)
 	estimator_aid_source1d_s _aid_src_sideslip{};
+#endif // CONFIG_EKF2_SIDESLIP
 
 	estimator_aid_source2d_s _aid_src_fake_pos{};
 	estimator_aid_source1d_s _aid_src_fake_hgt{};
@@ -725,12 +735,30 @@ private:
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
 
+#if defined(CONFIG_EKF2_AIRSPEED)
+	// control fusion of air data observations
+	void controlAirDataFusion(const imuSample &imu_delayed);
+
 	void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const;
 	void fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src);
 
+	void stopAirspeedFusion();
+
+	// 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);
+
+	// perform a limited reset of the wind state covariances
+	void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+	// control fusion of synthetic sideslip observations
+	void controlBetaFusion(const imuSample &imu_delayed);
+
 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
 	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
+#endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	// control fusion of multi-rotor drag specific force observations
@@ -956,12 +984,6 @@ private:
 	void runMagAndMagDeclFusions(const Vector3f &mag);
 	void run3DMagAndDeclFusions(const Vector3f &mag);
 
-	// control fusion of air data observations
-	void controlAirDataFusion(const imuSample &imu_delayed);
-
-	// control fusion of synthetic sideslip observations
-	void controlBetaFusion(const imuSample &imu_delayed);
-
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
@@ -1021,13 +1043,6 @@ private:
 
 	// perform a reset of the wind states and related covariances
 	void resetWind();
-
-	// 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);
-
-	// perform a limited reset of the wind state covariances
-	void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
-
 	void resetWindToZero();
 
 	// check that the range finder data is continuous
@@ -1067,8 +1082,6 @@ private:
 		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
 	}
 
-	void stopAirspeedFusion();
-
 	void stopGpsFusion();
 	void stopGpsPosFusion();
 	void stopGpsVelFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -693,14 +693,18 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		hgt = NAN;
 	}
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	// return the airspeed fusion innovation test ratio
 	tas = sqrtf(_aid_src_airspeed.test_ratio);
+#endif // CONFIG_EKF2_AIRSPEED
 
 	// return the terrain height innovation test ratio
 	hagl = sqrtf(_hagl_test_ratio);
 
+#if defined(CONFIG_EKF2_SIDESLIP)
 	// return the synthetic sideslip innovation test ratio
 	beta = sqrtf(_aid_src_sideslip.test_ratio);
+#endif // CONFIG_EKF2_SIDESLIP
 }
 
 // return a bitmask integer that describes which state estimates are valid
@@ -775,11 +779,18 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 
 	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
 
-	const bool airDataAiding = _control_status.flags.wind &&
+	bool airDataAiding = false;
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+	airDataAiding = _control_status.flags.wind &&
 				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
 				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
 
 	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
+#else
+	_control_status.flags.wind_dead_reckoning = false;
+#endif // CONFIG_EKF2_AIRSPEED
+
 	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
 
 	if (!_control_status.flags.inertial_dead_reckoning) {
@@ -1156,3 +1167,26 @@ void Ekf::resetGpsDriftCheckFilters()
 	_gps_vertical_position_drift_rate_m_s = NAN;
 	_gps_filtered_horizontal_velocity_m_s = NAN;
 }
+
+void Ekf::resetWind()
+{
+#if defined(CONFIG_EKF2_AIRSPEED)
+	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
+		resetWindUsingAirspeed(_airspeed_sample_delayed);
+		return;
+	}
+#endif // CONFIG_EKF2_AIRSPEED
+
+	resetWindToZero();
+}
+
+void Ekf::resetWindToZero()
+{
+	ECL_INFO("reset wind to zero");
+
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
+}

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -50,7 +50,9 @@ EstimatorInterface::~EstimatorInterface()
 	delete _mag_buffer;
 	delete _baro_buffer;
 	delete _range_buffer;
+#if defined(CONFIG_EKF2_AIRSPEED)
 	delete _airspeed_buffer;
+#endif // CONFIG_EKF2_AIRSPEED
 	delete _flow_buffer;
 	delete _ext_vision_buffer;
 #if defined(CONFIG_EKF2_DRAG_FUSION)
@@ -243,6 +245,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 	}
 }
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
 	if (!_initialised) {
@@ -277,6 +280,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_AIRSPEED
 
 void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
@@ -523,10 +527,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
 	}
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	// using airspeed
 	if (_params.arsp_thr > FLT_EPSILON) {
 		max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_AIRSPEED
 
 	// mag mode
 	if (_params.mag_fusion_type != MagFuseType::NONE) {
@@ -678,9 +684,11 @@ void EstimatorInterface::print_status()
 		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	if (_airspeed_buffer) {
 		printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_AIRSPEED
 
 	if (_flow_buffer) {
 		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());

src/modules/ekf2/EKF/estimator_interface.h

@@ -90,7 +90,9 @@ public:
 
 	void setBaroData(const baroSample &baro_sample);
 
+#if defined(CONFIG_EKF2_AIRSPEED)
 	void setAirspeedData(const airspeedSample &airspeed_sample);
+#endif // CONFIG_EKF2_AIRSPEED
 
 	void setRangeData(const rangeSample &range_sample);
 
@@ -291,7 +293,9 @@ protected:
 	// measurement samples capturing measurements on the delayed time horizon
 	gpsSample _gps_sample_delayed{};
 	sensor::SensorRangeFinder _range_sensor{};
+#if defined(CONFIG_EKF2_AIRSPEED)
 	airspeedSample _airspeed_sample_delayed{};
+#endif // CONFIG_EKF2_AIRSPEED
 	flowSample _flow_sample_delayed{};
 	extVisionSample _ev_sample_prev{};
 	RangeFinderConsistencyCheck _rng_consistency_check;
@@ -348,7 +352,9 @@ protected:
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 	RingBuffer<rangeSample> *_range_buffer{nullptr};
+#if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
+#endif // CONFIG_EKF2_AIRSPEED
 	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};