HTE: decrease sensitivity with speed

VTOL planes are getting lift from the wing when flying in MC mode at
high speed. They (and some other drones) also get extra drag when
climbing and descending at high speed, corrupting the hover thrust
estimate.

To avoid this, two speed thresholds (vertical and horizontal) are defined
above which the sensitivity of the estimator is decreased by linearly
increasing the observation noise.

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -168,9 +168,13 @@ void MulticopterHoverThrustEstimator::Run()
 				// Inform the hover thrust estimator about the measured vertical
 				// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
 				// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
-				if (fabsf(local_pos.vz) < 2.f) {
-					_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
-				}
+				const float meas_noise_coeff_z = fmaxf((fabsf(local_pos.vz) - _param_hte_vz_thr.get()) + 1.f, 1.f);
+				const float meas_noise_coeff_xy = fmaxf((matrix::Vector2f(local_pos.vx,
+									local_pos.vy).norm() - _param_hte_vxy_thr.get()) + 1.f,
+									1.f);
+
+				_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
+				_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
 
 				bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -121,6 +121,8 @@ private:
 		(ParamFloat<px4::params::HTE_HT_NOISE>) _param_hte_ht_noise,
 		(ParamFloat<px4::params::HTE_ACC_GATE>) _param_hte_acc_gate,
 		(ParamFloat<px4::params::HTE_HT_ERR_INIT>) _param_hte_ht_err_init,
+		(ParamFloat<px4::params::HTE_VXY_THR>) _param_hte_vxy_thr,
+		(ParamFloat<px4::params::HTE_VZ_THR>) _param_hte_vz_thr,
 		(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover
 	)
 };

src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c

@@ -81,3 +81,35 @@ PARAM_DEFINE_FLOAT(HTE_ACC_GATE, 3.0);
  * @group Hover Thrust Estimator
  */
 PARAM_DEFINE_FLOAT(HTE_HT_ERR_INIT, 0.1);
+
+/**
+ * Horizontal velocity threshold for sensitivity reduction
+ *
+ * Above this speed, the measurement noise is linearly increased
+ * to reduce the sensitivity of the estimator from biased measurement.
+ *
+ * Set to a low value on vehicles with large lifting surfaces.
+ *
+ * @decimal 1
+ * @min 1.0
+ * @max 20.0
+ * @unit m/s
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_VXY_THR, 10.0);
+
+/**
+ * Vertical velocity threshold for sensitivity reduction
+ *
+ * Above this speed, the measurement noise is linearly increased
+ * to reduce the sensitivity of the estimator from biased measurement.
+ *
+ * Set to a low value on vehicles affected by air drag when climbing or descending.
+ *
+ * @decimal 1
+ * @min 1.0
+ * @max 10.0
+ * @unit m/s
+ * @group Hover Thrust Estimator
+ */
+PARAM_DEFINE_FLOAT(HTE_VZ_THR, 2.0);

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp

@@ -88,7 +88,7 @@ inline float ZeroOrderHoverThrustEkf::computeH(const float thrust) const
 
 inline float ZeroOrderHoverThrustEkf::computeInnovVar(const float H) const
 {
-	const float R = _acc_var;
+	const float R = _acc_var * _acc_var_scale;
 	const float P = _state_var;
 	return math::max(H * P * H + R, R);
 }

src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp

@@ -81,6 +81,7 @@ public:
 	void setHoverThrust(float hover_thrust) { _hover_thr = math::constrain(hover_thrust, 0.1f, 0.9f); }
 	void setProcessNoiseStdDev(float process_noise) { _process_var = process_noise * process_noise; }
 	void setMeasurementNoiseStdDev(float measurement_noise) { _acc_var = measurement_noise * measurement_noise; }
+	void setMeasurementNoiseScale(float scale) { _acc_var_scale = scale * scale; }
 	void setHoverThrustStdDev(float hover_thrust_noise) { _state_var = hover_thrust_noise * hover_thrust_noise; }
 	void setAccelInnovGate(float gate_size) { _gate_size = gate_size; }
 
@@ -98,6 +99,7 @@ private:
 	float _state_var{0.01f}; ///< Initial hover thrust uncertainty variance (thrust^2)
 	float _process_var{12.5e-6f}; ///< Hover thrust process noise variance (thrust^2/s^2)
 	float _acc_var{5.f}; ///< Acceleration variance (m^2/s^3)
+	float _acc_var_scale{1.f}; ///< Multiplicator of the measurement variance, used to decrease sensivity
 	float _dt{0.02f};
 
 	float _innov{0.f}; ///< Measurement innovation (m/s^2)