From 904ed57abaf0d989711e22e968d0b0a0739b53de Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 3 Sep 2021 15:20:09 +0200 Subject: [PATCH] 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. --- .../MulticopterHoverThrustEstimator.cpp | 10 ++++-- .../MulticopterHoverThrustEstimator.hpp | 2 ++ .../hover_thrust_estimator_params.c | 32 +++++++++++++++++++ .../zero_order_hover_thrust_ekf.cpp | 2 +- .../zero_order_hover_thrust_ekf.hpp | 2 ++ 5 files changed, 44 insertions(+), 4 deletions(-) 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)