ekf: run rng consistency check only when not horizontally moving

The check assumes a non-moving terrain height
PX4 · Mar 21, 2022 · 3b02475 · 3b02475
1 parent 51d2f05
commit 3b02475
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
@@ -140,7 +140,11 @@ void Ekf::controlFusionModes()
 			const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
-			_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
+
+			// 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.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
+			}
 		}
 
 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();