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.

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

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -48,7 +48,7 @@ public:
 	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 @@ private:
 
 	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;
 

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 {