Range finder kinematic consistency check

msg/estimator_innovations.msg

@@ -31,6 +31,7 @@ float32[2] drag		# drag specific force innovation (m/sec**2) and innovation vari
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)
 float32 hagl		# height of ground innovation (m) and innovation variance (m**2)
+float32 hagl_rate	# height of ground rate innovation (m/s) and innovation variance ((m/s)**2)
 
 # The innovation test ratios are scalar values. In case the field is a vector,
 # the test ratio will be put in the first component of the vector.

msg/estimator_status_flags.msg

@@ -35,6 +35,7 @@ bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declare
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
+bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/CMakeLists.txt

@@ -66,6 +66,7 @@ px4_add_module(
 		EKF/mag_control.cpp
 		EKF/mag_fusion.cpp
 		EKF/optflow_fusion.cpp
+		EKF/range_finder_consistency_check.cpp
 		EKF/sensor_range_finder.cpp
 		EKF/sideslip_fusion.cpp
 		EKF/terrain_estimator.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -51,6 +51,7 @@ add_library(ecl_EKF
 	mag_control.cpp
 	mag_fusion.cpp
 	optflow_fusion.cpp
+	range_finder_consistency_check.cpp
 	sensor_range_finder.cpp
 	sideslip_fusion.cpp
 	terrain_estimator.cpp

src/modules/ekf2/EKF/common.h

@@ -307,6 +307,7 @@ struct parameters {
 	float range_aid_innov_gate{1.0f}; 	///< gate size used for innovation consistency checks for range aid fusion
 	float range_valid_quality_s{1.0f};	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
+	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
 
 	// vision position fusion
 	float ev_vel_innov_gate{3.0f};		///< vision velocity fusion innovation consistency gate size (STD)
@@ -493,6 +494,7 @@ union filter_control_status_u {
 		uint32_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
 		uint32_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 		uint32_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
+		uint32_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -140,7 +140,15 @@ 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());
+
+			// 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.setGate(_params.range_kin_consistency_gate);
+				_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();
 	}
 
 	if (_flow_buffer) {
@@ -827,7 +835,7 @@ void Ekf::checkVerticalAccelerationHealth()
 void Ekf::controlHeightFusion()
 {
 	checkRangeAidSuitability();
-	const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();
+	const bool do_range_aid = (_params.range_aid == 1) && _is_range_aid_suitable;
 
 	switch (_params.vdist_sensor_type) {
 	default:

src/modules/ekf2/EKF/ekf.h

@@ -128,6 +128,10 @@ class Ekf final : public EstimatorInterface
 	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
 	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
 
+	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
+	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
+	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+
 	// get the state vector at the delayed time horizon
 	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
 
@@ -899,7 +903,6 @@ class Ekf final : public EstimatorInterface
 
 	// determine if flight condition is suitable to use range finder instead of the primary height sensor
 	void checkRangeAidSuitability();
-	bool isRangeAidSuitable() const { return _is_range_aid_suitable; }
 
 	// set control flags to use baro height
 	void setControlBaroHeight();
@@ -930,8 +933,9 @@ class Ekf final : public EstimatorInterface
 
 	void updateGroundEffect();
 
-	// return an estimation of the GPS altitude variance
+	// return an estimation of the sensor altitude variance
 	float getGpsHeightVariance();
+	float getRngHeightVariance() const;
 
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1361,6 +1361,14 @@ void Ekf::updateBaroHgtBias()
 	}
 }
 
+float Ekf::getRngHeightVariance() const
+{
+	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+	const float var = sq(_params.range_noise) + dist_dependant_var;
+	const float var_sat = fmaxf(var, 0.001f);
+	return var_sat;
+}
+
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -63,6 +63,7 @@
 #include "common.h"
 #include "RingBuffer.h"
 #include "imu_down_sampler.hpp"
+#include "range_finder_consistency_check.hpp"
 #include "sensor_range_finder.hpp"
 #include "utils.hpp"
 
@@ -297,6 +298,8 @@ class EstimatorInterface
 	extVisionSample _ev_sample_delayed_prev{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 
+	RangeFinderConsistencyCheck _rng_consistency_check;
+
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
 
 	// Sensor limitations

src/modules/ekf2/EKF/range_finder_consistency_check.cpp

@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file range_finder_consistency_check.cpp
+ */
+
+#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)
+{
+	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
+
+	if ((_time_last_update_us == 0)
+	    || (dt < 0.001f) || (dt > 0.5f)) {
+		_time_last_update_us = time_us;
+		_dist_bottom_prev = dist_bottom;
+		return;
+	}
+
+	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
+	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
+
+	const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	_innov_var = var + vz_var;
+
+	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
+	_test_ratio = normalized_innov_sq / (_gate * _gate);
+	_signed_test_ratio_lpf.setParameters(dt, _signed_test_ratio_tau);
+	const float signed_test_ratio = matrix::sign(_innov) * _test_ratio;
+	_signed_test_ratio_lpf.update(signed_test_ratio);
+
+	updateConsistency(vz, time_us);
+
+	_time_last_update_us = time_us;
+	_dist_bottom_prev = dist_bottom;
+}
+
+void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
+{
+	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
+		_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)) {
+			_is_kinematically_consistent = true;
+		}
+	}
+}

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file range_finder_consistency_check.hpp
+ * @brief Compute statistical tests of the range finder data
+ * using the estimated velocity as a reference in order to detect sensor faults
+ */
+
+#pragma once
+
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+class RangeFinderConsistencyCheck final
+{
+public:
+	RangeFinderConsistencyCheck() = default;
+	~RangeFinderConsistencyCheck() = default;
+
+	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
+
+	void setGate(float gate) { _gate = gate; }
+
+	float getTestRatio() const { return _test_ratio; }
+	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
+	float getInnov() const { return _innov; }
+	float getInnovVar() const { return _innov_var; }
+	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
+
+private:
+	void updateConsistency(float vz, uint64_t time_us);
+
+	uint64_t _time_last_update_us{};
+	float _dist_bottom_prev{};
+
+	float _test_ratio{};
+	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
+	float _gate{.2f};
+	float _innov{};
+	float _innov_var{};
+
+	bool _is_kinematically_consistent{true};
+	uint64_t _time_last_inconsistent_us{};
+
+	static constexpr float _signed_test_ratio_tau = 2.f;
+
+	static constexpr float _min_vz_for_valid_consistency = .5f;
+	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+};

src/modules/ekf2/EKF/terrain_estimator.cpp

@@ -95,9 +95,9 @@ void Ekf::controlHaglRngFusion()
 	}
 
 	if (_range_sensor.isDataHealthy()) {
-		const bool continuing_conditions_passing = _control_status.flags.in_air;
+		const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent();
 		//const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height
-		const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData();
+		const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData() && (_rng_consistency_check.getTestRatio() < 1.f);
 
 		_time_last_healthy_rng_data = _time_last_imu;
 

src/modules/ekf2/EKF2.cpp

@@ -119,6 +119,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
 	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
+	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
 	_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
 	_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
 	_param_ekf2_of_n_min(_params->flow_noise),
@@ -838,6 +839,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnov(innovations.airspeed);
 	_ekf.getBetaInnov(innovations.beta);
 	_ekf.getHaglInnov(innovations.hagl);
+	_ekf.getHaglRateInnov(innovations.hagl_rate);
 	// Not yet supported
 	innovations.aux_vvel = NAN;
 
@@ -878,6 +880,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
 	_ekf.getBetaInnovRatio(test_ratios.beta);
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
+	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
 
@@ -902,6 +905,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getAirspeedInnovVar(variances.airspeed);
 	_ekf.getBetaInnovVar(variances.beta);
 	_ekf.getHaglInnovVar(variances.hagl);
+	_ekf.getHaglRateInnovVar(variances.hagl_rate);
 	// Not yet supported
 	variances.aux_vvel = NAN;
 
@@ -1301,6 +1305,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
+		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/EKF2.hpp

@@ -435,6 +435,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 		_param_ekf2_rng_a_igate,	///< gate size used for innovation consistency checks for range aid fusion (STD)
 		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>)
 		_param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
+		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>)
+		_param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD)
 
 		// vision estimate fusion
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>)

src/modules/ekf2/ekf2_params.c

@@ -1114,6 +1114,23 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);
 */
 PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f);
 
+/**
+ * Gate size used for range finder kinematic consistency check
+ *
+ * To be used, the time derivative of the distance sensor measurements projected on the vertical axis
+ * needs to be statistically consistent with the estimated vertical velocity of the drone.
+ *
+ * Decrease this value to make the filter more robust against range finder faulty data (stuck, reflections, ...).
+ *
+ * Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE) before tuning this gate.
+ *
+ * @group EKF2
+ * @unit SD
+ * @min 0.1
+ * @max 5.0
+*/
+PARAM_DEFINE_FLOAT(EKF2_RNG_K_GATE, 1.0f);
+
 /**
  * Gate size for vision velocity estimate fusion
  *