From fc8be4b48c1c48e30709bd75a5b41fe242af191c Mon Sep 17 00:00:00 2001
From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Date: Thu, 21 Sep 2023 17:40:36 +0200
Subject: [PATCH] ekf2 rng kin: allow check to become true during horizontal
 motion

Even if there is some horizontal motion, a passing check should be
accepted as the terrain can be flat. However, the vehicle must not be
moving horizontally to invalidate the consistency as a change in terrain
can make the kinematic check temporarily fail.
---
 .../EKF/range_finder_consistency_check.cpp    | 20 +++++++++++++++----
 .../EKF/range_finder_consistency_check.hpp    |  3 ++-
 src/modules/ekf2/EKF/range_height_control.cpp |  8 ++++----
 3 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp
index 0d8daee7dea9..453da560b421 100644
--- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp
+++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp
@@ -37,8 +37,12 @@
 
 #include "range_finder_consistency_check.hpp"
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
 {
+	if (horizontal_motion) {
+		_time_last_horizontal_motion = time_us;
+	}
+
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
 	if ((_time_last_update_us == 0)
@@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 
 void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 {
+	if (fabsf(vz) < _min_vz_for_valid_consistency) {
+		// We can only check consistency if there is vertical motion
+		return;
+	}
+
 	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		_is_kinematically_consistent = false;
-		_time_last_inconsistent_us = time_us;
+		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
+			_is_kinematically_consistent = false;
+			_time_last_inconsistent_us = time_us;
+		}
 
 	} else {
-		if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if (_test_ratio < 1.f
+		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
 			_is_kinematically_consistent = true;
 		}
 	}
diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp
index 4237c3f43723..d031e12d975b 100644
--- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp
+++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp
@@ -48,7 +48,7 @@ class RangeFinderConsistencyCheck final
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
 
 	void setGate(float gate) { _gate = gate; }
 
@@ -72,6 +72,7 @@ class RangeFinderConsistencyCheck final
 
 	bool _is_kinematically_consistent{true};
 	uint64_t _time_last_inconsistent_us{};
+	uint64_t _time_last_horizontal_motion{};
 
 	static constexpr float _signed_test_ratio_tau = 2.f;
 
diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp
index be3c26db7b5c..a9841bcc76fb 100644
--- a/src/modules/ekf2/EKF/range_height_control.cpp
+++ b/src/modules/ekf2/EKF/range_height_control.cpp
@@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
-			// Run the kinematic consistency check when not moving horizontally
-			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
-			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P.trace<2>(State::vel.idx), 0.1f))) {
+			if (_control_status.flags.in_air) {
+				const bool horizontal_motion = _control_status.flags.fixed_wing
+								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
 
 				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 				const float var = sq(_params.range_noise) + dist_dependant_var;
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), _time_delayed_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
 			}
 
 		} else {