EKF: Enable compensation for static pressure positional error (#7264)

* msg: add reporting of multi rotor drag fusion

* ekf2: add reporting of multi rotor drag fusion

* ekf2: Add parameters required to tune multi-rotor wind estimation

* ekf2: Add correction for static pressure position error

* ekf2: Use correct air density for position error corrections

* ekf2: fix parameter documentation error

* ekf2: Add separate forward and reverse position error correction factors

* ekf2: Fix formatting and parameter descriptions

* ekf2: Improve comments

msg/ekf2_innovations.msg

@@ -13,3 +13,5 @@ float32 beta_innov_var		# synthetic sideslip innovation variance
 float32[2] flow_innov_var	# flow innovation variance
 float32 hagl_innov_var          # innovation variance from the terrain estimator for the height above ground level measurement (m^2)
 float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m)
+float32[2] drag_innov		# drag specific force innovation (m/s/s)
+float32[2] drag_innov_var	# drag specific force innovation variance (m/s/s)**2

src/modules/ekf2/ekf2_main.cpp

@@ -82,6 +82,7 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/sensor_baro.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -183,6 +184,9 @@ private:
 	math::LowPassFilter2p _lp_pitch_rate;	///< Low pass filter applied to pitch rates published on the control_state message
 	math::LowPassFilter2p _lp_yaw_rate;	///< Low pass filter applied to yaw rates published on the control_state message
 
+	// Used to correct baro data for positional errors
+	Vector3f _vel_body_wind = {};	// XYZ velocity relative to wind in body frame (m/s)
+
 	Ekf _ekf;
 
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)
@@ -319,6 +323,22 @@ private:
 	control::BlockParamFloat
 	_mag_bias_alpha;	///< maximum fraction of the learned magnetometer bias that is saved at each disarm
 
+	// Multi-rotor drag specific force fusion
+	control::BlockParamExtFloat
+	_drag_noise;	///< observation noise variance for drag specific force measurements (m/sec**2)**2
+	control::BlockParamExtFloat _bcoef_x;		///< ballistic coefficient along the X-axis (kg/m**2)
+	control::BlockParamExtFloat _bcoef_y;		///< ballistic coefficient along the Y-axis (kg/m**2)
+
+	// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
+	// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
+	control::BlockParamFloat _aspd_max;		///< upper limit on airspeed used for correction  (m/s**2)
+	control::BlockParamFloat
+	_K_pstatic_coef_xp;	///< static pressure position error coefficient along the positive X body axis
+	control::BlockParamFloat
+	_K_pstatic_coef_xn;	///< static pressure position error coefficient along the negative X body axis
+	control::BlockParamFloat _K_pstatic_coef_y;	///< static pressure position error coefficient along the Y body axis
+	control::BlockParamFloat _K_pstatic_coef_z;	///< static pressure position error coefficient along the Z body axis
+
 };
 
 Ekf2::Ekf2():
@@ -430,8 +450,15 @@ Ekf2::Ekf2():
 	_mag_bias_z(this, "EKF2_MAGBIAS_Z", false),
 	_mag_bias_id(this, "EKF2_MAGBIAS_ID", false),
 	_mag_bias_saved_variance(this, "EKF2_MAGB_VREF", false),
-	_mag_bias_alpha(this, "EKF2_MAGB_K", false)
-
+	_mag_bias_alpha(this, "EKF2_MAGB_K", false),
+	_drag_noise(this, "EKF2_DRAG_NOISE", false, _params->drag_noise),
+	_bcoef_x(this, "EKF2_BCOEF_X", false, _params->bcoef_x),
+	_bcoef_y(this, "EKF2_BCOEF_Y", false, _params->bcoef_y),
+	_aspd_max(this, "EKF2_ASPD_MAX", false),
+	_K_pstatic_coef_xp(this, "EKF2_PCOEF_XP", false),
+	_K_pstatic_coef_xn(this, "EKF2_PCOEF_XN", false),
+	_K_pstatic_coef_y(this, "EKF2_PCOEF_Y", false),
+	_K_pstatic_coef_z(this, "EKF2_PCOEF_Z", false)
 {
 
 }
@@ -463,6 +490,7 @@ void Ekf2::run()
 	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
+	int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 
 	px4_pollfd_struct_t fds[1] = {};
 	fds[0].fd = sensors_sub;
@@ -484,6 +512,8 @@ void Ekf2::run()
 	vehicle_attitude_s ev_att = {};
 	vehicle_status_s vehicle_status = {};
 	sensor_selection_s sensor_selection = {};
+	sensor_baro_s sensor_baro = {};
+	sensor_baro.pressure = 1013.5; // initialise pressure to sea level
 
 	while (!should_exit()) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -683,7 +713,36 @@ void Ekf2::run()
 				uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;
 
 				if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
+					// take mean across sample period
 					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
+
+					// estimate air density assuming typical 20degC ambient temperature
+					orb_copy(ORB_ID(sensor_baro), sensor_baro_sub, &sensor_baro);
+					const float pressure_to_density = 100.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+					float rho = pressure_to_density * sensor_baro.pressure;
+					_ekf.set_air_density(rho);
+
+					// calculate static pressure error = Pmeas - Ptruth
+					// model position error sensitivity as a body fixed ellipse with different scale in the positive and negtive X direction
+					float max_airspeed_sq = _aspd_max.get();
+					max_airspeed_sq *= max_airspeed_sq;
+					float K_pstatic_coef_x;
+
+					if (_vel_body_wind(0) >= 0.0f) {
+						K_pstatic_coef_x = _K_pstatic_coef_xp.get();
+
+					} else {
+						K_pstatic_coef_x = _K_pstatic_coef_xn.get();
+					}
+
+					float pstatic_err = 0.5f * rho * (K_pstatic_coef_x * fminf(_vel_body_wind(0) * _vel_body_wind(0), max_airspeed_sq) +
+									  _K_pstatic_coef_y.get() * fminf(_vel_body_wind(1) * _vel_body_wind(1), max_airspeed_sq) +
+									  _K_pstatic_coef_z.get() * fminf(_vel_body_wind(2) * _vel_body_wind(2), max_airspeed_sq));
+
+					// correct baro measurement using pressure error estimate and assuming sea level gravity
+					balt_data_avg += pstatic_err / (rho * 9.80665f);
+
+					// push to estimator
 					_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);
 					_balt_time_ms_last_used = balt_time_ms;
 					_balt_time_sum_ms = 0;
@@ -825,6 +884,12 @@ void Ekf2::run()
 				ctrl_state.y_vel = v_b(1);
 				ctrl_state.z_vel = v_b(2);
 
+				// Calculate velocity relative to wind in body frame
+				float velNE_wind[2] = {};
+				_ekf.get_wind_velocity(velNE_wind);
+				v_n(0) -= velNE_wind[0];
+				v_n(1) -= velNE_wind[1];
+				_vel_body_wind = R_to_body * v_n;
 
 				// Local Position NED
 				float position[3];
@@ -856,23 +921,40 @@ void Ekf2::run()
 				ctrl_state.airspeed_valid = false;
 
 				// use estimated velocity for airspeed estimate
+				// TODO move this out of the estimators and put it into a dedicated air data consolidation algorithm
 				if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
 					// use measured airspeed
 					if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && now - airspeed.timestamp < 1e6
 					    && airspeed.timestamp > 0) {
 						ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
 						ctrl_state.airspeed_valid = true;
+
+					} else {
+						// This airspeed mode requires a measurement which we no longer have, so wind relative speed
+						// is used as a surrogate and the validity is set to false.
+						ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2));
+						ctrl_state.airspeed_valid = false;
+
 					}
 
 				} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
 					if (_ekf.local_position_is_valid()) {
-						ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
+						// This airspeed mode uses an estimate which is calculated from the wind relative speed
+						// TODO modify the ecl EKF to provide a boolean validity with the wind speed estimate and
+						// use that to set the validity of the estimated airspeed.
+						ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2));
 						ctrl_state.airspeed_valid = true;
+
 					}
 
 				} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
-					// do nothing, airspeed has been declared as non-valid above, controllers
-					// will handle this assuming always trim airspeed
+					// This airspeed mode has disabled airspeed use and controllers will handle this.
+					// We still return wind relative speed as a surrogate and set the validity to zero.
+					if (_ekf.local_position_is_valid()) {
+						ctrl_state.airspeed = sqrtf(v_n(0) * v_n(0) + v_n(1) * v_n(1) + v_n(2) * v_n(2));
+						ctrl_state.airspeed_valid = false;
+
+					}
 				}
 
 				// publish control state data
@@ -1161,6 +1243,7 @@ void Ekf2::run()
 				_ekf.get_beta_innov(&innovations.beta_innov);
 				_ekf.get_flow_innov(&innovations.flow_innov[0]);
 				_ekf.get_hagl_innov(&innovations.hagl_innov);
+				_ekf.get_drag_innov(&innovations.drag_innov[0]);
 
 				_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
 				_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
@@ -1169,6 +1252,7 @@ void Ekf2::run()
 				_ekf.get_beta_innov_var(&innovations.beta_innov_var);
 				_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
 				_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
+				_ekf.get_drag_innov_var(&innovations.drag_innov_var[0]);
 
 				_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
 

src/modules/ekf2/ekf2_params.c

@@ -541,15 +541,17 @@ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
  * 2 : Set to true to inhibit IMU bias estimation
  * 3 : Set to true to enable vision position fusion
  * 4 : Set to true to enable vision yaw fusion
+ * 5 : Set to true to enable multi-rotor drag specific force fusion
  *
  * @group EKF2
  * @min 0
- * @max 28
+ * @max 63
  * @bit 0 use GPS
  * @bit 1 use optical flow
  * @bit 2 inhibit IMU bias estimation
  * @bit 3 vision position fusion
  * @bit 4 vision yaw fusion
+ * @bit 5 multi-rotor drag fusion
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
 
@@ -1056,3 +1058,97 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f);
  * @max 5.0
  */
 PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);
+
+/**
+ * Specific drag force observation noise variance used by the multi-rotor specific drag force model.
+ * Increasing it makes the multi-rotor wind estimates adjust more slowly.
+ *
+ * @group EKF2
+ * @min 0.5
+ * @max 10.0
+ * @unit (m/sec**2)**2
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f);
+
+/**
+ * X-axis ballistic coefficient used by the multi-rotor specific drag force model.
+ * This should be adjusted to minimise variance of the X-axis drag specific force innovation sequence.
+ *
+ * @group EKF2
+ * @min 1.0
+ * @max 100.0
+ * @unit kg/m**2
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 25.0f);
+
+/**
+ * Y-axis ballistic coefficient used by the multi-rotor specific drag force model.
+ * This should be adjusted to minimise variance of the Y-axis drag specific force innovation sequence.
+ *
+ * @group EKF2
+ * @min 1.0
+ * @max 100.0
+ * @unit kg/m**2
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 25.0f);
+
+/**
+ * Upper limit on airspeed along individual axes used to correct baro for position error effects
+ *
+ * @group EKF2
+ * @min 5.0
+ * @max 50.0
+ * @unit m/s
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_ASPD_MAX, 20.0f);
+
+/**
+ * Static pressure position error coefficient for the positive X axis
+ * This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis.
+ * If the baro height estimate rises during forward flight, then this will be a negative number.
+ *
+ * @group EKF2
+ * @min -0.5
+ * @max 0.5
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_XP, 0.0f);
+
+/**
+ * Static pressure position error coefficient for the negative X axis.
+ * This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis.
+ * If the baro height estimate rises during backwards flight, then this will be a negative number.
+ *
+ * @group EKF2
+ * @min -0.5
+ * @max 0.5
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_XN, 0.0f);
+
+/**
+ * Pressure position error coefficient for the Y axis.
+ * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the Y body axis.
+ * If the baro height estimate rises during sideways flight, then this will be a negative number.
+ *
+ * @group EKF2
+ * @min -0.5
+ * @max 0.5
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_Y, 0.0f);
+
+/**
+ * Static pressure position error coefficient for the Z axis.
+ * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the Z body axis.
+ *
+ * @group EKF2
+ * @min -0.5
+ * @max 0.5
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_PCOEF_Z, 0.0f);