From 4c03f0bc5087596315d0991d43d310c2aad260e0 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 23 Mar 2022 15:50:58 +0100 Subject: [PATCH] ekf: make range finder kin consistency gate tunable by parameter --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 1 + .../ekf2/EKF/range_finder_consistency_check.hpp | 6 ++++-- src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/ekf2/ekf2_params.c | 15 +++++++++++++++ 6 files changed, 24 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index a964e2ea21..d940d40220 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -307,6 +307,7 @@ struct parameters { float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data + float range_kin_consistency_gate{0.2f}; ///< gate size used by the range finder kinematic consistency check // vision position fusion float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 64f3e226e3..dff7f37d07 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -143,6 +143,7 @@ void Ekf::controlFusionModes() // Run the kinematic consistency check when not moving horizontally if ((sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); _rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu); } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index 792f54af2b..5745213d5f 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -49,6 +49,8 @@ public: void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void setGate(float gate) { _vel_bottom_gate = gate; } + float getTestRatio() const { return _vel_bottom_test_ratio; } float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); } float getInnov() const { return _vel_bottom_innov; } @@ -63,6 +65,7 @@ private: float _vel_bottom_test_ratio{}; AlphaFilter _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data + float _vel_bottom_gate{.2f}; float _vel_bottom_innov{}; float _vel_bottom_innov_var{}; @@ -70,8 +73,7 @@ private: uint64_t _time_last_inconsistent_us{}; static constexpr float _vel_bottom_signed_test_ratio_tau = 2.f; - static constexpr float _vel_bottom_gate = 0.1f; - static constexpr float _min_vz_for_valid_consistency = 0.5f; + static constexpr float _min_vz_for_valid_consistency = .5f; static constexpr uint64_t _consistency_hyst_time_us = 1e6; }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6a300f1f2d..d455dc7d17 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -119,6 +119,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), + _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 7072968d9f..fe7fbdca63 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -435,6 +435,8 @@ private: _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) (ParamExtFloat) _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + (ParamExtFloat) + _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) // vision estimate fusion (ParamInt) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 80f63d5327..f7565b2106 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1114,6 +1114,21 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); */ PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f); +/** + * Gate size used for range finder kinematic consistency check + * + * To be used, the time derivative of the distance sensor measurements projected on the vertical axis + * needs to be statistically consistent with the estimated vertical velocity of the drone. + * + * Decrease this value to make the filter more robust against range finder faulty data (stuck, reflections, ...). + * + * @group EKF2 + * @unit SD + * @min 0.01 + * @max 5.0 +*/ +PARAM_DEFINE_FLOAT(EKF2_RNG_K_GATE, 0.2f); + /** * Gate size for vision velocity estimate fusion *