diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 4074916ff3..8f4d8b808c 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/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); diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 6f6280e115..2e88f6a8f5 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -121,6 +121,8 @@ private: (ParamFloat) _param_hte_ht_noise, (ParamFloat) _param_hte_acc_gate, (ParamFloat) _param_hte_ht_err_init, + (ParamFloat) _param_hte_vxy_thr, + (ParamFloat) _param_hte_vz_thr, (ParamFloat) _param_mpc_thr_hover ) }; diff --git a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c b/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c index 47d7f6ea38..3e1af502d0 100644 --- a/src/modules/mc_hover_thrust_estimator/hover_thrust_estimator_params.c +++ b/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); diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp index 94c13cad7b..64fec9d074 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.cpp +++ b/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); } diff --git a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp b/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp index 43d4a173a5..1c944879d7 100644 --- a/src/modules/mc_hover_thrust_estimator/zero_order_hover_thrust_ekf.hpp +++ b/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)