ekf: improve rng consistency check

To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1

To pass from valid to invalid:
- low-passed signed test ratio >= 1

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -51,15 +51,23 @@ public:
 
 	float getTestRatio() const { return _vel_bottom_test_ratio; }
 	float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); }
-	bool isKinematicallyConsistent() const { return _vel_bottom_signed_test_ratio_lpf.getState() < 1.f; }
+	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
 
 private:
+	void updateConsistency(float vz, float time_s);
+
 	float _time_last_update_s{};
 	float _dist_bottom_prev{};
 
 	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
 
+	bool _is_kinematically_consistent{true};
+	float _time_last_inconsistent{};
+
 	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 _consistency_hyst_time = 1.f;
 };