New filter for distance sensor validation

msg/EstimatorStatusFlags.msg

@@ -47,6 +47,7 @@ bool cs_rng_terrain             # 39 - true if we are fusing range finder data f
 bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain
 bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
+bool cs_rng_kin_unknown      	# 43 - true when the range finder kinematic consistency check is not running
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp

@@ -36,56 +36,119 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "ekf_derivation/generated/range_validation_filter.h"
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
-		bool horizontal_motion, uint64_t time_us)
+using namespace matrix;
+
+void RangeFinderConsistencyCheck::init(const float &z, const float &z_var, const float &dist_bottom,
+				       const float &dist_bottom_var)
 {
-	if (horizontal_motion) {
-		_time_last_horizontal_motion = time_us;
-	}
+	float p[4] = {z_var, 0.f, 0.f, z_var + dist_bottom_var};
+	_P = Matrix<float, RangeFilter::size, RangeFilter::size>(p);
+	sym::RangeValidationFilter(&_H);
+	_x(RangeFilter::z.idx) = z;
+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
+	_initialized = true;
+	_state = _test_ratio_lpf.getState() < 1.f ? KinematicState::UNKNOWN : KinematicState::INCONSISTENT;
+	_test_ratio_lpf.reset(2.f);
+	_t_since_first_sample = 0.f;
+	_test_ratio_lpf.setAlpha(0.2f);
+}
 
+void RangeFinderConsistencyCheck::update(const float &z, const float &z_var, const float &vz, const float &vz_var,
+		const float &dist_bottom, const float &dist_bottom_var, const 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)) {
+	if (_time_last_update_us == 0 || dt > 1.f) {
 		_time_last_update_us = time_us;
-		_dist_bottom_prev = dist_bottom;
+		init(z, z_var, dist_bottom, dist_bottom_var);
 		return;
 	}
 
-	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
-	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
+	_time_last_update_us = time_us;
 
-	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
-	const float var = 2.f * dist_bottom_var / (dt * dt);
-	_innov_var = var + vz_var;
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_P(0, 0) += dt * dt * vz_var + 0.001f;
+	_P(1, 1) += terrain_process_noise;
 
-	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);
+	const Vector2f measurements(z, dist_bottom);
 
-	updateConsistency(vz, time_us);
+	Vector2f Kv{1.f, 0.f};
+	Vector2f test_ratio{0.f, 0.f};
+	Vector2f R{z_var, dist_bottom_var};
+	Vector2f y;
 
-	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
-}
+	for (int i = 0; i < 2 ; i++) {
+		y = measurements - _H * _x;
+		Vector2f H = _H.row(i);
+		_innov_var = (H.transpose() * _P * H + R(i))(0, 0);
+		Kv(RangeFilter::terrain.idx) = _P(RangeFilter::terrain.idx, i) / _innov_var;
 
-void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
-{
-	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
+		Vector2f PH = _P.row(i);
+
+		for (int u = 0; u < RangeFilter::size; u++) {
+			for (int v = 0; v < RangeFilter::size; v++) {
+				_P(u, v) -= Kv(u) * PH(v);
+			}
 		}
 
-	} 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;
+		PH = _P.col(i);
+
+		for (int u = 0; u < RangeFilter::size; u++) {
+			for (int v = 0; v <= u; v++) {
+				_P(u, v) = _P(u, v) - PH(u) * Kv(v) + Kv(u) * R(i) * Kv(v);
+				_P(v, u) = _P(u, v);
+			}
+		}
+
+		test_ratio(i) = fminf(sq(y(i)) / sq(_gate), 4.f);
+
+		if (i == (int)RangeFilter::z.idx && test_ratio(1) > 1.f) {
+			Kv(1) = 0.f;
 		}
+
+		_x = _x + Kv * y;
 	}
+
+	_innov = y(RangeFilter::terrain.idx);
+	_test_ratio_lpf.update(sign(_innov) * test_ratio(1));
+
+	// start the consistency check after 1s
+	if (_t_since_first_sample + dt > 1.0f) {
+		_t_since_first_sample = 2.0f;
+
+		if (abs(_test_ratio_lpf.getState()) < 1.f) {
+			const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
+
+			if (!horizontal_motion && vertical_motion) {
+				_state = KinematicState::CONSISTENT;
+
+			} else {
+				_state = KinematicState::UNKNOWN;
+			}
+
+		} else {
+			_t_since_first_sample = 0.f;
+			_state = KinematicState::INCONSISTENT;
+		}
+
+	} else {
+		_t_since_first_sample += dt;
+	}
+}
+
+void RangeFinderConsistencyCheck::run(const float &z, const float &vz,
+				      const matrix::SquareMatrix<float, estimator::State::size> &P,
+				      const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us)
+{
+	const float z_var = P(estimator::State::pos.idx + 2, estimator::State::pos.idx + 2);
+	const float vz_var = P(estimator::State::vel.idx + 2, estimator::State::vel.idx + 2);
+
+	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
+		_last_posD_reset_count = current_posD_reset_count;
+		init(z, z_var, dist_bottom, dist_bottom_var);
+	}
+
+	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
 }

src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp

@@ -41,43 +41,63 @@
 #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 
 #include <mathlib/math/filter/AlphaFilter.hpp>
+#include <ekf_derivation/generated/state.h>
+
 
 class RangeFinderConsistencyCheck final
 {
 public:
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
+	enum class KinematicState : int {
+		INCONSISTENT = 0,
+		CONSISTENT = 1,
+		UNKNOWN = 2
+	};
 
-	void setGate(float gate) { _gate = gate; }
+	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
+	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
+	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT; }
+	void setGate(const float gate) { _gate = gate; }
+	void run(const float &z, const float &vz, const matrix::SquareMatrix<float, estimator::State::size> &P,
+		 const float &dist_bottom, const float &dist_bottom_var, uint64_t time_us);
+	void reset()
+	{
+		_state = (_initialized && _state == KinematicState::CONSISTENT) ? KinematicState::UNKNOWN : _state;
+		_initialized = false;
+	}
 
-	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; }
+	uint8_t current_posD_reset_count{0};
+	float terrain_process_noise{0.0f};
+	bool horizontal_motion{false};
 
 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{};
-	uint64_t _time_last_horizontal_motion{};
-
-	static constexpr float _signed_test_ratio_tau = 2.f;
+	void update(const float &z, const float &z_var, const float &vz, const float &vz_var, const float &dist_bottom,
+		    const float &dist_bottom_var, const uint64_t &time_us);
+	void init(const float &z, const float &z_var, const float &dist_bottom, const float &dist_bottom_var);
+	matrix::SquareMatrix<float, 2> _P{};
+	matrix::SquareMatrix<float, 2> _H{};
+	matrix::Vector2f _x{};
+	bool _initialized{false};
+	float _innov{0.f};
+	float _innov_var{0.f};
+	uint64_t _time_last_update_us{0};
+	AlphaFilter<float> _test_ratio_lpf{};
+	float _gate{1.f};
+	KinematicState _state{KinematicState::UNKNOWN};
+	float _t_since_first_sample{0.f};
+	uint8_t _last_posD_reset_count{0};
+};
 
-	static constexpr float _min_vz_for_valid_consistency = .5f;
-	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+namespace RangeFilter
+{
+static constexpr estimator::IdxDof z{0, 1};
+static constexpr estimator::IdxDof terrain{1, 1};
+static constexpr uint8_t size{2};
 };
 
 #endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -56,6 +56,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 		_range_sensor.setMaxFogDistance(_params.rng_fog);
+		_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
@@ -66,30 +67,39 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
 			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_var = getRngVar();
+				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
 
-				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
-				const float var = sq(_params.range_noise) + dist_dependant_var;
+				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
 
-				_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), horizontal_motion, imu_sample.time_us);
+				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion
+				    && _rng_consistency_check.isNotKinematicallyInconsistent()) {
+					_rng_consistency_check.reset();
+				}
+
+				_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
+				_rng_consistency_check.run(_gpos.altitude(), _state.vel(2), P, _range_sensor.getRange(), dist_var, imu_sample.time_us);
 			}
 
 		} else {
 			// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
 			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air
-			    && _range_sensor.isRegularlySendingData()
+			if (_range_sensor.isRegularlySendingData()
 			    && _range_sensor.isDataReady()) {
+				if (!_control_status.flags.in_air) {
+
+					_range_sensor.setRange(_params.rng_gnd_clearance);
+					_range_sensor.setValidity(true); // bypass the checks
 
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true); // bypass the checks
+				} else {
+					_rng_consistency_check.reset();
+				}
 			}
 		}
 
 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+		_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+							&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
 	} else {
 		return;
@@ -107,13 +117,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
+				&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
-
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
@@ -137,7 +146,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -163,7 +173,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -200,14 +211,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else {
+					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 				}
 
 			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stoppPing %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
 				stopRngTerrFusion();
 			}
@@ -221,7 +232,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected) {
+					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -268,11 +279,9 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.range_noise)
-		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
-		       0.f);
+	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)