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.cpp

@@ -59,6 +59,21 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 	const float signed_test_ratio = matrix::sign(innov) * _vel_bottom_test_ratio;
 	_vel_bottom_signed_test_ratio_lpf.update(signed_test_ratio);
 
+	updateConsistency(vz, time_s);
+
 	_time_last_update_s = time_s;
 	_dist_bottom_prev = dist_bottom;
 }
+
+void RangeFinderConsistencyCheck::updateConsistency(float vz, float time_s)
+{
+	if (fabsf(_vel_bottom_signed_test_ratio_lpf.getState()) >= 1.f) {
+		_is_kinematically_consistent = false;
+		_time_last_inconsistent = time_s;
+
+	} else {
+		if (fabsf(vz) > _min_vz_for_valid_consistency && _vel_bottom_test_ratio < 1.f && ((time_s - _time_last_inconsistent) > _consistency_hyst_time)) {
+			_is_kinematically_consistent = true;
+		}
+	}
+}

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