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