ekf: make range finder kin consistency gate tunable by parameter

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)

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);
 			}
 		}

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<float> _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;
 };

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),

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<px4::params::EKF2_RNG_QLTY_T>)
 		_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<px4::params::EKF2_RNG_K_GATE>)
+		_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
 
 		// vision estimate fusion
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>)

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
  *