From 079a5e92ba06b1e4064c23b664bb6c367f97e213 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 21 Mar 2022 15:32:10 +0100 Subject: [PATCH] ekf: run rng consistency check only when not horizontally moving The check assumes a non-moving terrain height --- src/modules/ekf2/EKF/control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 6a1f5d95a3..64f3e226e3 100644 --- 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();