kill of sensor_range_finder abstraction completely

ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500

@@ -49,3 +49,9 @@ param set-default SIM_GZ_EC_MAX4 1000
 
 param set-default MPC_THR_HOVER 0.60
 param set-default NAV_DLL_ACT 2
+
+# DEBUGGING: ekf range fusion
+# default, ekf replay, cv+avoid, high rate sensors
+param set-default SDLOG_PROFILE 2179
+# sim distance on ground
+param set-default EKF2_MIN_RNG 0.177

src/modules/ekf2/CMakeLists.txt

@@ -213,7 +213,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
 		EKF/aid_sources/range_finder/range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
-		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 	list(APPEND EKF_MODULE_PARAMS params_range_finder.yaml)
 endif()

src/modules/ekf2/EKF/CMakeLists.txt

@@ -126,7 +126,6 @@ if(CONFIG_EKF2_RANGE_FINDER)
 		aid_sources/range_finder/range_finder_consistency_check.cpp
 		aid_sources/range_finder/range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
-		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
 

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

@@ -1,84 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. 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 Sensor.hpp
- * Abstract class for sensors
- *
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
- *
- */
-
-#ifndef EKF_SENSOR_HPP
-#define EKF_SENSOR_HPP
-
-#include <cstdint>
-
-namespace estimator
-{
-namespace sensor
-{
-
-class Sensor
-{
-public:
-	virtual ~Sensor() {};
-
-	/*
-	 * run sanity checks on the current data
-	 * this has to be called immediately after
-	 * setting new data
-	 */
-	virtual void runChecks() {};
-
-	/*
-	 * return true if the sensor is healthy
-	 */
-	virtual bool isHealthy() const = 0;
-
-	/*
-	 * return true if the delayed sample is healthy
-	 * and can be fused in the estimator
-	 */
-	virtual bool isDataHealthy() const = 0;
-
-	/*
-	 * return true if the sensor data rate is
-	 * stable and high enough
-	 */
-	virtual bool isRegularlySendingData() const = 0;
-};
-
-} // namespace sensor
-} // namespace estimator
-#endif // !EKF_SENSOR_HPP

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

@@ -36,56 +36,131 @@
  */
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "ekf_derivation/generated/range_validation_filter_h.h"
+#include "ekf_derivation/generated/range_validation_filter_P_init.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;
-	}
+	printf("RangeFinderConsistencyCheck::init\n");
 
+	_p = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
+	_ht = sym::RangeValidationFilterH<float>();
+
+	_x(RangeFilter::z.idx) = z;
+	_x(RangeFilter::terrain.idx) = z - dist_bottom;
+	_initialized = true;
+	_test_ratio = 0.f;
+	_t_since_first_sample = 0.f;
+}
+
+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 (dt > 1.f) {
 		_time_last_update_us = time_us;
-		_dist_bottom_prev = dist_bottom;
+		_initialized = false;
+		return;
+
+	} else if (!_initialized) {
+		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
-
-	// 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;
-
-	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);
-
+	// prediction step
 	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
-}
+	_x(RangeFilter::z.idx) -= dt * vz;
+	_p(RangeFilter::z.idx, RangeFilter::z.idx) += dt * dt * vz_var;
+	_p(RangeFilter::terrain.idx, RangeFilter::terrain.idx) += _terrain_process_noise;
 
-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;
+	// iterate through both measurements (z and dist_bottom)
+	const Vector2f measurements{z, dist_bottom};
+
+	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
+
+		// dist_bottom
+		Vector2f H = _ht;
+		float R = dist_bottom_var;
+
+		// z, direct state measurement
+		if (measurement_idx == 0) {
+			H(RangeFilter::z.idx) = 1.f;
+			H(RangeFilter::terrain.idx) = 0.f;
+			R = z_var;
 		}
 
-	} 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;
+		// residual
+		const float measurement_pred = H * _x;
+		const float y = measurements(measurement_idx) - measurement_pred;
+
+		// for H as col-vector:
+		// innovation variance S = H^T * P * H + R
+		// kalman gain K = P * H / S
+		const float S = (H.transpose() * _p * H + R)(0, 0);
+		Vector2f K = (_p * H / S);
+
+		if (measurement_idx == 0) {
+			K(RangeFilter::z.idx) = 1.f;
+
+		} else if (measurement_idx == 1) {
+			_innov = y;
+			const float test_ratio = fminf(sq(y) / (sq(_gate) * S), 4.f); // limit to 4 to limit sensitivity to outliers
+			_test_ratio = sign(_innov) * test_ratio;
 		}
+
+		// update step
+		_x(RangeFilter::z.idx)       += K(RangeFilter::z.idx) * y;
+		_x(RangeFilter::terrain.idx) += K(RangeFilter::terrain.idx) * y;
+
+		// covariance update with Joseph form:
+		// P = (I - K H) P (I - K H)^T + K R K^T
+		Matrix2f I;
+		I.setIdentity();
+		Matrix2f IKH = I - K.multiplyByTranspose(H);
+		_p = IKH * _p * IKH.transpose() + (K * R).multiplyByTranspose(K);
 	}
+
+	evaluateState(dt, vz, vz_var);
+}
+
+void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
+{
+	// TODO: magic test ratio
+	if (fabsf(_test_ratio) > 1.f) {
+		_t_since_first_sample = 0.f;
+		_state = KinematicState::INCONSISTENT;
+		return;
+	}
+
+	_state = KinematicState::CONSISTENT;
+}
+
+bool RangeFinderConsistencyCheck::isKinematicallyConsistent()
+{
+	return _state == KinematicState::CONSISTENT;
+}
+
+void RangeFinderConsistencyCheck::reset()
+{
+	if (_initialized && _state == KinematicState::CONSISTENT) {
+		_state = KinematicState::UNKNOWN;
+	}
+
+	_initialized = false;
+}
+
+void RangeFinderConsistencyCheck::run(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)
+{
+	if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
+		printf("kcc run, resetting\n");
+		_last_posD_reset_count = current_posD_reset_count;
+		_initialized = false;
+	}
+
+	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

@@ -40,7 +40,9 @@
 #ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 #define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP
 
-#include <mathlib/math/filter/AlphaFilter.hpp>
+#include <mathlib/math/Functions.hpp>
+
+using namespace math;
 
 class RangeFinderConsistencyCheck final
 {
@@ -48,36 +50,56 @@ 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 _test_ratio; }
 
-	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; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
+	bool isKinematicallyConsistent();
+	void setGate(const float gate) { _gate = gate; }
+	void run(float z, float z_var, float vz, float vz_var,
+		 float dist_bottom, float dist_bottom_var, uint64_t time_us);
+	void set_terrain_process_noise(float terrain_process_noise) { _terrain_process_noise = terrain_process_noise; }
+	void reset();
+
+	uint8_t current_posD_reset_count{0};
 
 private:
-	void updateConsistency(float vz, uint64_t time_us);
 
-	uint64_t _time_last_update_us{};
-	float _dist_bottom_prev{};
+	void update(float z, float z_var, float vz, float vz_var, float dist_bottom,
+		    float dist_bottom_var, uint64_t time_us);
+	void init(float z, float z_var, float dist_bottom, float dist_bottom_var);
+	void evaluateState(float dt, float vz, float vz_var);
+	float _terrain_process_noise{0.0f};
+	matrix::SquareMatrix<float, 2> _p{};
+	matrix::Vector2f _ht{};
+	matrix::Vector2f _x{};
+	bool _initialized{false};
+	float _innov{0.f};
+	float _innov_var{0.f};
+	uint64_t _time_last_update_us{0};
+	static constexpr float time_constant{1.f};
 
-	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{};
+	float _test_ratio{0.f};
 
-	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;
-
-	static constexpr float _min_vz_for_valid_consistency = .5f;
-	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+	float _gate{1.0f};
+	KinematicState _state{KinematicState::UNKNOWN};
+	float _t_since_first_sample{0.f};
+	uint8_t _last_posD_reset_count{0};
+	static constexpr float _t_to_init{1.f};
 };
 
+namespace RangeFilter
+{
+struct IdxDof { unsigned idx; unsigned dof; };
+static constexpr IdxDof z{0, 1};
+static constexpr 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

@@ -42,237 +42,276 @@
 
 void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
-	static constexpr const char *HGT_SRC_NAME = "RNG";
-
-	bool rng_data_ready = false;
-
-	if (_range_buffer) {
-		// Get range data from buffer and check validity
-		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
-		_range_sensor.setDataReadiness(rng_data_ready);
-
-		// update range sensor angle parameters in case they have changed
-		_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
-		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
-		_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
-
-		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
-
-		if (_range_sensor.isDataHealthy()) {
-			// correct the range data for position offset relative to the IMU
-			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());
-
-			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.ekf2_rng_sfe * _range_sensor.getDistBottom());
-				const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
-
-				_rng_consistency_check.setGate(_params.ekf2_rng_k_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);
-			}
-
-		} else {
-			// If we are supposed to be using range finder data but 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()
-			    && _range_sensor.isDataReady()) {
-
-				_range_sensor.setRange(_params.ekf2_min_rng);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
-		}
-
-		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
-
-	} else {
+	if (!_range_buffer) {
 		return;
 	}
 
-	auto &aid_src = _aid_src_rng_hgt;
+	// TODO: why isn't this being done anywhere?
+	_aid_src_rng_hgt.fused = false;
 
-	if (rng_data_ready && _range_sensor.getSampleAddress()) {
+	// Pop rangefinder measurement from buffer of samples into active sample
+	rangeSample sample = {};
 
-		updateRangeHagl(aid_src);
-		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
+	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
+		// no new sample available, check if we've timed out
+		bool had_sample = _range_time_last_good_sample > 0;
+		bool timed_out = imu_sample.time_us > _range_time_last_good_sample + 200'000;
 
-		const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
-				|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
-				&& _control_status.flags.tilt_align
-				&& measurement_valid
-				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
-
-		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.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-						      && isConditionalRangeAidSuitable();
-
-		const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
-					  && (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
-
-		if (_control_status.flags.rng_hgt) {
-			if (!(do_conditional_range_aid || do_range_aid)) {
-				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
-				stopRngHgtFusion();
-			}
-
-		} else {
-			if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
-				if (do_conditional_range_aid) {
-					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
-					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-
-					_control_status.flags.rng_hgt = true;
-					stopRngTerrFusion();
-
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
-						resetTerrainToRng(aid_src);
-						resetAidSourceStatusZeroInnovation(aid_src);
-					}
-
-				} else if (do_range_aid) {
-					// Range finder is the primary height source, the ground is now the datum used
-					// to compute the local vertical position
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-
-					_information_events.flags.reset_hgt_to_rng = true;
-					resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
-					_state.terrain = 0.f;
-					resetAidSourceStatusZeroInnovation(aid_src);
-					_control_status.flags.rng_hgt = true;
-					stopRngTerrFusion();
-
-					aid_src.time_last_fuse = imu_sample.time_us;
-				}
-
-			} else {
-				if (do_conditional_range_aid || do_range_aid) {
-					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) {
-						resetTerrainToRng(aid_src);
-						resetAidSourceStatusZeroInnovation(aid_src);
-					}
-				}
-			}
+		if (had_sample && timed_out) {
+			stopRangeAltitudeFusion("sensor timed out");
+			stopRangeTerrainFusion("sensor timed out");
 		}
 
-		if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
-			if (continuing_conditions_passing) {
+		return;
+	}
 
-				fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+	// Quality checks
+	if (sample.quality == 0) {
+		if (_control_status.flags.in_air) {
+			// Disable fusion after 1s of bad quality
+			uint64_t elapsed = sample.time_us - _range_time_last_good_sample;
 
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
-
-				if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
-					// All height sources are failing
-					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
-
-					_information_events.flags.reset_hgt_to_rng = true;
-					resetAltitudeTo(aid_src.observation - _state.terrain);
-					resetAidSourceStatusZeroInnovation(aid_src);
-
-					// reset vertical velocity if no valid sources available
-					if (!isVerticalVelocityAidingActive()) {
-						resetVerticalVelocityToZero();
-					}
-
-					aid_src.time_last_fuse = imu_sample.time_us;
-
-				} else if (is_fusion_failing) {
-					// Some other height source is still working
-					if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) {
-						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
-						stopRngHgtFusion();
-						stopRngTerrFusion();
-
-					} else {
-						resetTerrainToRng(aid_src);
-						resetAidSourceStatusZeroInnovation(aid_src);
-					}
-				}
-
-			} else {
-				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
-				stopRngHgtFusion();
-				stopRngTerrFusion();
+			if (elapsed > 1'000'000) {
+				stopRangeAltitudeFusion("sensor quality bad");
+				stopRangeTerrainFusion("sensor quality bad");
 			}
 
+			return;
+
 		} else {
-			if (starting_conditions_passing) {
-				if (_control_status.flags.opt_flow_terrain) {
-					if (!aid_src.innovation_rejected) {
-						_control_status.flags.rng_terrain = true;
-						fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
-					}
-
-				} else {
-					if (aid_src.innovation_rejected) {
-						resetTerrainToRng(aid_src);
-						resetAidSourceStatusZeroInnovation(aid_src);
-					}
-
-					_control_status.flags.rng_terrain = true;
-				}
-			}
+			// While on ground fake a measurement at ground level if the signal quality is bad
+			sample.quality = 100;
+			sample.range = sample.min_distance;
+			_range_time_last_good_sample = sample.time_us;
 		}
 
-	} else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain)
-		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
-		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
-		stopRngHgtFusion();
-		stopRngTerrFusion();
+	} else {
+		_range_time_last_good_sample = sample.time_us;
+	}
+
+	float cos_theta = _R_to_earth(2, 2);
+	bool tilt_ok = cos_theta > _params.range_cos_max_tilt;
+	bool range_ok = sample.range <= sample.max_distance && sample.range >= sample.min_distance;
+
+	// If tilt or value are outside of bounds -- throw away measurement
+	if (!tilt_ok || !range_ok) {
+		stopRangeAltitudeFusion("pre checks failed");
+		stopRangeTerrainFusion("pre checks failed");
+		return;
+	}
+
+	// TODO: refactor into apply_body_offset()
+	// Correct the range data for position offset relative to the IMU
+	const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+
+	sample.range = sample.range + pos_offset_earth(2) / cos_theta; // rotate into earth frame
+
+	// TODO: refactor into consintency_filter_update() that runs consistency status
+	// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
+	_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
+
+	// rotate into world frame
+	float sample_corrected = sample.range * cos_theta;
+
+	const float z = _gpos.altitude();
+	const float vz = _state.vel(2);
+	const float dist = sample_corrected;
+
+	const float dist_var = 0.05;
+	const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
+	const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
+
+	// Run the kinematic consistency check
+	_rng_consistency_check.run(z, z_var, vz, vz_var, dist, dist_var, imu_sample.time_us);
+
+	// Track kinematic consistency
+	_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+
+	// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
+	updateRangeHagl(_aid_src_rng_hgt, sample_corrected, sample.time_us);
+
+	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
+		printf("INFINIFY OBSERVATION INVALID\n");
+	}
+
+	// Fuse Range data as Primary height source
+	// NOTE: terrain is not estimated in this mode
+	if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+		fuseRangeAsHeightSource();
+
+	} else {
+		// Fuse Range data as aiding height source (Primary GPS or Baro)
+		fuseRangeAsHeightAiding();
 	}
 }
 
-void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+void Ekf::fuseRangeAsHeightAiding()
 {
-	const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
-	const float measurement_variance = getRngVar();
+	const char *kNotKinematicallyConsistentText = "not kinematically consistent";
+	const char *kConditionsFailingText = "conditions failing";
 
-	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+	// Stop fusion if rangefinder kinematic consistency fails
+	if (!_control_status.flags.rng_kin_consistent) {
+		stopRangeTerrainFusion(kNotKinematicallyConsistentText);
+		stopRangeAltitudeFusion(kNotKinematicallyConsistentText);
+		return;
+	}
 
-	const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
-	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
-			      measurement,                               // observation
-			      measurement_variance,                      // observation variance
-			      getHagl() - measurement,                   // innovation
-			      innovation_variance,                       // innovation variance
-			      innov_gate);                               // innovation gate
+	//// TERRAIN FUSION ////
 
-	// z special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
-		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
+	// Fuse Range into Terrain if:
+	// - kinematically consistent (hagl_rate < 1)
+
+	// Start fusion
+	if (!_control_status.flags.rng_terrain) {
+		printf("START RNG Terrain fusion\n");
+		_control_status.flags.rng_terrain = true;
+
+		// We must reset terrain to range before we start fusing again, otherwise there
+		// can be a delta between RNG terrain estimate and previous, which will cause a
+		// discontinuity in the lpos.z state
+		printf("resetting terrain to range\n");
+		resetTerrainToRng(_aid_src_rng_hgt);
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+	}
+
+
+	//// ALTITUDE FUSION ////
+
+	bool range_aid_conditional = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::CONDITIONAL;
+	bool range_aid_enabled = (RngCtrl)_params.ekf2_rng_ctrl == RngCtrl::ENABLED;
+
+	bool range_aid_conditions_passed = rangeAidConditionsPassed();
+
+	bool do_range_aid = range_aid_enabled || (range_aid_conditional && range_aid_conditions_passed);
+
+	// Fuse Range into Altitude if:
+	// - passes range_aid_conditionalchecks
+	// - kinematically consistent
+	if (do_range_aid) {
+
+		if (!_control_status.flags.rng_hgt) {
+			printf("START RNG Altitude fusion\n");
+			_control_status.flags.rng_hgt = true;
+
+			// Reset altitude to rangefinder if on ground
+			if (!_control_status.flags.in_air) {
+				printf("GND resetting altitude to range\n");
+				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+			}
+
+			// TODO: review for correctness
+			if (_aid_src_rng_hgt.innovation_rejected) {
+				// Reset aid source
+				printf("resetting aid source\n");
+				resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+			}
+		}
+
+	} else {
+		stopRangeAltitudeFusion(kConditionsFailingText);
+	}
+
+	// If we make it here, fuse
+	fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
+}
+
+// TODO: remove this mode entirely
+void Ekf::fuseRangeAsHeightSource()
+{
+	// When primary height source is RANGE, we always fuse it
+	// I don't think conditional range aid mode makes sense in the context of RANGE = primary
+
+	// Fusion init logic
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+
+		_height_sensor_ref = HeightSensor::RANGE;
+		_information_events.flags.reset_hgt_to_rng = true;
+
+		// Reset aid source innovation
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+
+		// Reset altitude to RANGE
+		resetAltitudeTo(_aid_src_rng_hgt.observation, _aid_src_rng_hgt.observation_variance);
+		_control_status.flags.rng_hgt = true;
+
+		// Cannot have terrain estimate fusion while RANGE is primary
+		stopRangeTerrainFusion("Cannot have terrain estimate fusion while RANGE is primary");
+		printf("initializing range as primary\n");
+		_state.terrain = 0.f;
+
+		// TODO: needed? It's set above in --> resetAidSourceStatusZeroInnovation()
+		// _aid_src_rng_hgt.time_last_fuse = imu_sample.time_us;
+	}
+
+	// TODO:
+	// When RNG is primary height source
+	if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
+		printf("RNG height fusion reset required, all height sources failing\n");
+
+		uint64_t timestamp = _aid_src_rng_hgt.timestamp;
+		_information_events.flags.reset_hgt_to_rng = true;
+		resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+
+		// reset vertical velocity if no valid sources available
+		if (!isVerticalVelocityAidingActive()) {
+			printf("resetting vertical velocity\n");
+			resetVerticalVelocityToZero();
+		}
+
+		_aid_src_rng_hgt.time_last_fuse = timestamp;
 	}
 }
 
-float Ekf::getRngVar() const
+bool Ekf::rangeAidConditionsPassed()
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.ekf2_rng_noise)
-		       + sq(_params.ekf2_rng_sfe * _range_sensor.getRange()),
-		       0.f);
+	bool is_in_range = getHagl() < _params.ekf2_rng_a_hmax;
+	// bool is_hagl_stable = _aid_src_rng_hgt.test_ratio < 1.f;
+	// bool is_horizontal_aiding_active = isHorizontalAidingActive();
+	// bool is_below_max_speed = is_horizontal_aiding_active && !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
+	bool is_below_max_speed = !_state.vel.xy().longerThan(_params.ekf2_rng_a_vmax);
+
+	// Require conditions passing for 1_s (same as kinematic consistency check)
+	bool conditions_passing = false;
+	// bool conditions_met = is_in_range && is_hagl_stable && is_below_max_speed;
+	bool conditions_met = is_in_range && is_below_max_speed;
+
+	if (conditions_met) {
+		if (!_rng_aid_conditions_valid) {
+			// Conditions just became valid
+			printf("RNG AID conditions valid\n");
+			_rng_aid_conditions_valid = true;
+			_time_rng_aid_conditions_valid = _time_latest_us;
+		}
+
+		if (_time_latest_us > _time_rng_aid_conditions_valid + 1'000'000) {
+			// Conditions have been valid for at least 1s
+			conditions_passing = true;
+		}
+
+	} else {
+
+		if (_rng_aid_conditions_valid) {
+			_rng_aid_conditions_valid = false;
+
+			// if (!is_hagl_stable) {
+			//  printf("!is_hagl_stable\n");
+			// }
+			// if (is_horizontal_aiding_active && !is_below_max_speed) {
+			if (!is_below_max_speed) {
+				printf("!is_below_max_speed\n");
+			}
+
+			if (!is_in_range) {
+				printf("!is_in_range\n");
+			}
+		}
+	}
+
+	return conditions_passing;
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
@@ -309,47 +348,48 @@ void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 	aid_src.time_last_fuse = _time_delayed_us;
 }
 
-bool Ekf::isConditionalRangeAidSuitable()
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src, float measurement, uint64_t time_us)
 {
-	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-	float range_hagl_max = _params.ekf2_rng_a_hmax;
-	float max_vel_xy = _params.ekf2_rng_a_vmax;
+	measurement = math::max(measurement, _params.ekf2_min_rng);
+	const float measurement_variance = sq(_params.ekf2_rng_noise) + sq(_params.ekf2_rng_sfe * measurement);
 
-	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
 
-	bool is_hagl_stable = (hagl_test_ratio < 1.f);
+	const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      time_us,                                   // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate
 
-	if (!_control_status.flags.rng_hgt) {
-		range_hagl_max = 0.7f * _params.ekf2_rng_a_hmax;
-		max_vel_xy = 0.7f * _params.ekf2_rng_a_vmax;
-		is_hagl_stable = (hagl_test_ratio < 0.01f);
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
 	}
-
-	const bool is_in_range = (getHagl() < range_hagl_max);
-
-	bool is_below_max_speed = true;
-
-	if (isHorizontalAidingActive()) {
-		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
-	}
-
-	return is_in_range && is_hagl_stable && is_below_max_speed;
 }
 
-void Ekf::stopRngHgtFusion()
+void Ekf::stopRangeAltitudeFusion(const char *reason)
 {
 	if (_control_status.flags.rng_hgt) {
+		printf("STOP RNG Altitude fusion: %s\n", reason);
+		_control_status.flags.rng_hgt = false;
 
 		if (_height_sensor_ref == HeightSensor::RANGE) {
 			_height_sensor_ref = HeightSensor::UNKNOWN;
 		}
-
-		_control_status.flags.rng_hgt = false;
 	}
 }
 
-void Ekf::stopRngTerrFusion()
+void Ekf::stopRangeTerrainFusion(const char *reason)
 {
-	_control_status.flags.rng_terrain = false;
+	if (_control_status.flags.rng_terrain) {
+		printf("STOP RNG Terrain fusion: %s\n", reason);
+		_control_status.flags.rng_terrain = false;
+	}
 }

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

@@ -1,171 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. 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 sensor_range_finder.cpp
- *
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
- *
- */
-
-#include <aid_sources/range_finder/sensor_range_finder.hpp>
-
-#include <lib/matrix/matrix/math.hpp>
-
-namespace estimator
-{
-namespace sensor
-{
-
-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
-{
-	updateSensorToEarthRotation(R_to_earth);
-	updateValidity(current_time_us);
-}
-
-void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
-{
-	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
-	// this is required for use of range finder and flow data
-	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
-}
-
-void SensorRangeFinder::updateValidity(uint64_t current_time_us)
-{
-	updateDtDataLpf(current_time_us);
-
-	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
-		_is_sample_valid = false;
-		_is_regularly_sending_data = false;
-		return;
-	}
-
-	_is_regularly_sending_data = true;
-
-	// Don't run the checks unless we have retrieved new data from the buffer
-	if (_is_sample_ready) {
-		_is_sample_valid = false;
-
-		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
-
-		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
-			return;
-		}
-
-		updateStuckCheck();
-		updateFogCheck(getDistBottom(), _sample.time_us);
-
-		if (!_is_stuck && !_is_blocked) {
-			_is_sample_valid = true;
-			_time_last_valid_us = _sample.time_us;
-		}
-	}
-}
-
-bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
-{
-	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
-}
-
-void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
-{
-	// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
-	float alpha = 0.5f * _dt_update;
-	_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
-
-	// Apply spike protection to the filter state.
-	_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
-}
-
-inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
-{
-	return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
-}
-
-inline bool SensorRangeFinder::isDataInRange() const
-{
-	return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
-}
-
-void SensorRangeFinder::updateStuckCheck()
-{
-	if (!isStuckDetectorEnabled()) {
-		// Stuck detector disabled
-		_is_stuck = false;
-		return;
-	}
-
-	// Check for "stuck" range finder measurements when range was not valid for certain period
-	// This handles a failure mode observed with some lidar sensors
-	if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {
-
-		// require a variance of rangefinder values to check for "stuck" measurements
-		if (_stuck_max_val - _stuck_min_val > _stuck_threshold) {
-			_stuck_min_val = 0.0f;
-			_stuck_max_val = 0.0f;
-			_is_stuck = false;
-
-		} else {
-			if (_sample.rng > _stuck_max_val) {
-				_stuck_max_val = _sample.rng;
-			}
-
-			if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) {
-				_stuck_min_val = _sample.rng;
-			}
-
-			_is_stuck = true;
-		}
-	}
-}
-
-void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
-{
-	if (_max_fog_dist > 0.f) {
-
-		const float median_dist = _median_dist.apply(dist_bottom);
-		const float factor = 2.f; // magic hardcoded factor
-
-		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
-			_is_blocked = true;
-
-		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
-			_is_blocked = false;
-		}
-
-		_prev_median_dist = median_dist;
-	}
-}
-
-} // namespace sensor
-} // namespace estimator

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

@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020-2023 PX4 Development Team. 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 sensor_range_finder.hpp
- * Range finder class containing all the required checks
- *
- * @author Mathieu Bresciani <brescianimathieu@gmail.com>
- *
- */
-#ifndef EKF_SENSOR_RANGE_FINDER_HPP
-#define EKF_SENSOR_RANGE_FINDER_HPP
-
-#include "Sensor.hpp"
-
-#include <matrix/math.hpp>
-#include <lib/mathlib/math/filter/MedianFilter.hpp>
-
-namespace estimator
-{
-namespace sensor
-{
-
-struct rangeSample {
-	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
-	float       rng{};      ///< range (distance to ground) measurement (m)
-	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
-};
-
-static constexpr uint64_t RNG_MAX_INTERVAL =
-	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
-
-class SensorRangeFinder : public Sensor
-{
-public:
-	SensorRangeFinder() = default;
-	~SensorRangeFinder() override = default;
-
-	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
-	bool isHealthy() const override { return _is_sample_valid; }
-	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
-	bool isDataReady() const { return _is_sample_ready; }
-	bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
-	bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
-
-	void setSample(const rangeSample &sample)
-	{
-		_sample = sample;
-		_is_sample_ready = true;
-	}
-
-	// This is required because of the ring buffer
-	// TODO: move the ring buffer here
-	rangeSample *getSampleAddress() { return &_sample; }
-
-	void setPitchOffset(float new_pitch_offset)
-	{
-		if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) {
-			_sin_pitch_offset = sinf(new_pitch_offset);
-			_cos_pitch_offset = cosf(new_pitch_offset);
-			_pitch_offset_rad = new_pitch_offset;
-		}
-	}
-
-	void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; }
-
-	void setLimits(float min_distance, float max_distance)
-	{
-		_rng_valid_min_val = min_distance;
-		_rng_valid_max_val = max_distance;
-	}
-
-	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
-
-	void setQualityHysteresis(float valid_quality_threshold_s)
-	{
-		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
-	}
-
-	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
-
-	void setRange(float rng) { _sample.rng = rng; }
-	float getRange() const { return _sample.rng; }
-
-	float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
-
-	void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
-	void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
-
-	float getValidMinVal() const { return _rng_valid_min_val; }
-	float getValidMaxVal() const { return _rng_valid_max_val; }
-
-	void setFaulty(bool faulty = true) { _is_faulty = faulty; }
-
-private:
-	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
-
-	void updateValidity(uint64_t current_time_us);
-	void updateDtDataLpf(uint64_t current_time_us);
-	bool isSampleOutOfDate(uint64_t current_time_us) const;
-	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
-	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
-	bool isDataInRange() const;
-	bool isQualityOk(uint64_t current_time_us) const;
-	void updateStuckCheck();
-	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
-
-	rangeSample _sample{};
-
-	bool _is_sample_ready{};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
-	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
-	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
-	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
-	bool _is_faulty{false};         ///< the sensor should not be used anymore
-
-	/*
-	 * Stuck check
-	 */
-	bool _is_stuck{};
-	float _stuck_threshold{0.1f};	///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
-	float _stuck_min_val{};		///< minimum value for new rng measurement when being stuck
-	float _stuck_max_val{};		///< maximum value for new rng measurement when being stuck
-
-	/*
-	 * Data regularity check
-	 */
-	static constexpr float _dt_update{0.01f}; 	///< delta time since last ekf update TODO: this should be a parameter
-	float _dt_data_lpf{};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
-
-	/*
-	 * Tilt check
-	 */
-	float _cos_tilt_rng_to_earth{1.f};		///< 2,2 element of the rotation matrix from sensor frame to earth frame
-	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 _pitch_offset_rad{3.14f}; 		///< range finder tilt rotation about the Y body axis
-	float _sin_pitch_offset{0.0f}; 		///< sine of the range finder tilt rotation about the Y body axis
-	float _cos_pitch_offset{-1.0f}; 		///< cosine of the range finder tilt rotation about the Y body axis
-
-	/*
-	 * Range check
-	 */
-	float _rng_valid_min_val{};	///< minimum distance that the rangefinder can measure (m)
-	float _rng_valid_max_val{};	///< maximum distance that the rangefinder can measure (m)
-
-	/*
-	 * Quality check
-	 */
-	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
-	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
-
-	/*
-	 * Fog check
-	 */
-	bool _is_blocked{false};
-	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
-	math::MedianFilter<float, 5> _median_dist{};
-	float _prev_median_dist{0.f};
-
-};
-
-} // namespace sensor
-} // namespace estimator
-#endif // !EKF_SENSOR_RANGE_FINDER_HPP

src/modules/ekf2/EKF/common.h

@@ -222,6 +222,14 @@ struct airspeedSample {
 	float       eas2tas{};          ///< equivalent to true airspeed factor
 };
 
+struct rangeSample {
+	uint64_t    time_us{};  // timestamp of the measurement (uSec)
+	float       range{};    // range (distance to ground) measurement (m)
+	int8_t      quality{};  // Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
+	float 		min_distance{};
+	float 		max_distance{};
+};
+
 struct flowSample {
 	uint64_t    time_us{};   ///< timestamp of the integration period midpoint (uSec)
 	Vector2f    flow_rate{}; ///< measured angular rate of the image about the X and Y body axes (rad/s), RH rotation is positive
@@ -419,7 +427,6 @@ struct parameters {
 	float ekf2_rng_delay{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float ekf2_rng_noise{0.1f};             ///< observation noise for range finder measurements (m)
 	float ekf2_rng_gate{5.0f};              ///< range finder fusion innovation consistency gate size (STD)
-	float ekf2_rng_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float ekf2_rng_sfe{0.0f};               ///< scaling from range measurement to noise (m/m)
 	float ekf2_rng_a_hmax{5.0f};            ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float ekf2_rng_a_vmax{1.0f};            ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)

src/modules/ekf2/EKF/covariance.cpp

@@ -105,8 +105,8 @@ void Ekf::initialiseCovariance()
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// use the ground clearance value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_min_rng));
+	// use the noise value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
 #endif // CONFIG_EKF2_TERRAIN
 }
 
@@ -227,6 +227,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel(
 						 1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		_rng_consistency_check.set_terrain_process_noise(terrain_process_noise);
+#endif // CONFIG_EKF2_RANGE_FINDER
 	}
 
 #endif // CONFIG_EKF2_TERRAIN

src/modules/ekf2/EKF/ekf.cpp

@@ -77,13 +77,6 @@ void Ekf::reset()
 	_state.terrain = -_gpos.altitude() + _params.ekf2_min_rng;
 #endif // CONFIG_EKF2_TERRAIN
 
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
-	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-	_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
-	_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
 
@@ -408,6 +401,11 @@ void Ekf::updateParameters()
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
 	_aux_global_position.updateParameters();
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+#if defined (CONFIG_EKF2_RANGE_FINDER)
+	// TODO: initialize static rangefinder parameters
+	_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
+
+#endif
 }
 
 template<typename T>

src/modules/ekf2/EKF/ekf.h

@@ -117,7 +117,7 @@ public:
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
 	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -765,9 +765,8 @@ private:
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
-	void updateRangeHagl(estimator_aid_source1d_s &aid_src);
+	void updateRangeHagl(estimator_aid_source1d_s &aid_src, float measurement, uint64_t time_us);
 	void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
-	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -779,9 +778,12 @@ private:
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
 	void controlRangeHaglFusion(const imuSample &imu_delayed);
+	void fuseRangeAsHeightSource();
+	void fuseRangeAsHeightAiding();
 	bool isConditionalRangeAidSuitable();
-	void stopRngHgtFusion();
-	void stopRngTerrFusion();
+	bool rangeAidConditionsPassed();
+	void stopRangeAltitudeFusion(const char *reason);
+	void stopRangeTerrainFusion(const char *reason);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -343,10 +343,10 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
-	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
+	const float rangefinder_hagl_min = _range_min_distance;
 
 	// Allow use of 90% of rangefinder maximum range to allow for angular motion
-	const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal();
+	const float rangefinder_hagl_max = 0.9f * _range_max_distance;
 
 	// TODO : calculate visual odometry limits
 	const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -283,7 +283,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
+void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
 	if (!_initialised) {
 		return;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
 
 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -301,6 +301,10 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 		}
 	}
 
+	// TODO: better way to do this?
+	_range_min_distance = range_sample.min_distance;
+	_range_max_distance = range_sample.max_distance;
+
 	const int64_t time_us = range_sample.time_us
 				- static_cast<int64_t>(_params.ekf2_rng_delay * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
@@ -308,7 +312,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {
 
-		sensor::rangeSample range_sample_new{range_sample};
+		rangeSample range_sample_new{range_sample};
 		range_sample_new.time_us = time_us;
 
 		_range_buffer->push(range_sample_new);

src/modules/ekf2/EKF/estimator_interface.h

@@ -68,7 +68,6 @@
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 # include "aid_sources/range_finder/range_finder_consistency_check.hpp"
-# include "aid_sources/range_finder/sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_GNSS)
@@ -113,15 +112,8 @@ public:
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const estimator::sensor::rangeSample &range_sample);
+	void setRangeData(const estimator::rangeSample &range_sample);
 
-	// set sensor limitations reported by the rangefinder
-	void set_rangefinder_limits(float min_distance, float max_distance)
-	{
-		_range_sensor.setLimits(min_distance, max_distance);
-	}
-
-	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -373,11 +365,19 @@ protected:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
+	RingBuffer<rangeSample> *_range_buffer {nullptr};
 	uint64_t _time_last_range_buffer_push{0};
 
-	sensor::SensorRangeFinder _range_sensor{};
-	RangeFinderConsistencyCheck _rng_consistency_check;
+	// Range aiding
+	bool _rng_aid_conditions_valid{};
+	uint64_t _time_rng_aid_conditions_valid{};
+	uint64_t _range_time_last_good_sample{};
+
+	// TODO: find a better way to store these?
+	float _range_min_distance{};
+	float _range_max_distance{};
+
+	RangeFinderConsistencyCheck _rng_consistency_check{};
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -721,6 +721,37 @@ def compute_gravity_z_innov_var_and_h(
 
     return (innov_var, H.T)
 
+def range_validation_filter_h() -> sf.Matrix:
+
+    state = Values(
+        z=sf.Symbol("z"),
+        terrain=sf.Symbol("terrain")
+    )
+    dist_bottom = state["z"] - state["terrain"]
+
+    H = sf.M21()
+    H[:, 0] = sf.V1(dist_bottom).jacobian(state.to_storage(), tangent_space=False).transpose()
+
+    return H
+
+def range_validation_filter_P_init(
+        z_var: sf.Scalar,
+        dist_bottom_var: sf.Scalar
+    ) -> sf.Matrix:
+
+    H = range_validation_filter_h().T
+    # enforce terrain to match the measurement
+    K = sf.V2(0, 1/H[1])
+    R = sf.V1(dist_bottom_var)
+
+    # dist_bottom = z - terrain
+    P = sf.M22.diag([z_var, z_var + dist_bottom_var])
+    I = sf.M22.eye()
+    # Joseph form
+    P = (I - K * H) * P * (I - K * H).T + K * R * K.T
+
+    return P
+
 print("Derive EKF2 equations...")
 generate_px4_function(predict_covariance, output_names=None)
 
@@ -752,5 +783,7 @@ generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_va
 generate_px4_function(compute_body_vel_innov_var_h, output_names=["innov_var", "Hx", "Hy", "Hz"])
 generate_px4_function(compute_body_vel_y_innov_var, output_names=["innov_var"])
 generate_px4_function(compute_body_vel_z_innov_var, output_names=["innov_var"])
+generate_px4_function(range_validation_filter_h, output_names=None)
+generate_px4_function(range_validation_filter_P_init, output_names=None)
 
 generate_px4_state(State, tangent_idx)

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h

@@ -0,0 +1,46 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_p_init
+ *
+ * Args:
+ *     z_var: Scalar
+ *     dist_bottom_var: Scalar
+ *
+ * Outputs:
+ *     res: Matrix22
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 2> RangeValidationFilterPInit(const Scalar z_var,
+                                                       const Scalar dist_bottom_var) {
+  // Total ops: 1
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 2> _res;
+
+  _res(0, 0) = z_var;
+  _res(1, 0) = z_var;
+  _res(0, 1) = z_var;
+  _res(1, 1) = dist_bottom_var + z_var;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h

@@ -0,0 +1,41 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: range_validation_filter_h
+ *
+ * Args:
+ *
+ * Outputs:
+ *     res: Matrix21
+ */
+template <typename Scalar>
+matrix::Matrix<Scalar, 2, 1> RangeValidationFilterH() {
+  // Total ops: 0
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  matrix::Matrix<Scalar, 2, 1> _res;
+
+  _res(0, 0) = 1;
+  _res(1, 0) = -1;
+
+  return _res;
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/terrain_control.cpp

@@ -45,8 +45,8 @@ void Ekf::initTerrain()
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.ekf2_min_rng;
 
-	// use the ground clearance value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_min_rng));
+	// use the sensor noise value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_rng_noise));
 }
 
 void Ekf::controlTerrainFakeFusion()
@@ -54,7 +54,6 @@ void Ekf::controlTerrainFakeFusion()
 	// If we are on ground, store the local position and time to use as a reference
 	if (!_control_status.flags.in_air) {
 		_last_on_ground_posD = -_gpos.altitude();
-		_control_status.flags.rng_fault = false;
 
 	} else if (!_control_status_prev.flags.in_air) {
 		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
@@ -78,14 +77,13 @@ void Ekf::updateTerrainValidity()
 {
 	bool valid_opt_flow_terrain = false;
 	bool valid_rng_terrain = false;
-	bool positive_hagl_var = false;
-	bool small_relative_hagl_var = false;
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
-	if (_control_status.flags.opt_flow_terrain
-	    && isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max)
-	   ) {
+	bool flow_terrain = _control_status.flags.opt_flow_terrain;
+	bool flow_recent = isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max);
+
+	if (flow_terrain && flow_recent) {
 		valid_opt_flow_terrain = true;
 	}
 
@@ -93,42 +91,34 @@ void Ekf::updateTerrainValidity()
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 
-	if (_control_status.flags.rng_terrain
-	    && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)
-	   ) {
+	bool rng_terrain = _control_status.flags.rng_terrain;
+	bool rng_recent = isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max);
+
+	if (rng_terrain && rng_recent) {
 		valid_rng_terrain = true;
 	}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+	bool small_relative_hagl_var = false;
+
 	if (_time_last_terrain_fuse != 0) {
 		// Assume being valid when the uncertainty is small compared to the height above ground
 		float hagl_var = INFINITY;
 		sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
 
-		positive_hagl_var = hagl_var > 0.f;
-
-		if (positive_hagl_var
-		    && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))
-		   ) {
+		// TODO: quantify this hagl_var check
+		if ((hagl_var > 0.f) && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))) {
 			small_relative_hagl_var = true;
 		}
 	}
 
 	const bool positive_hagl = getHagl() >= 0.f;
 
-	if (!_terrain_valid) {
-		// require valid RNG or optical flow (+valid variance) to initially consider terrain valid
-		if (positive_hagl
-		    && positive_hagl_var
-		    && (valid_rng_terrain
-			|| (valid_opt_flow_terrain && small_relative_hagl_var))
-		   ) {
-			_terrain_valid = true;
-		}
+	// TODO: review terrain validity requirements
+	// dist_bottom_valid == _terrain_valid
 
-	} else {
-		// terrain was previously valid, continue considering valid if variance is good
-		_terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var;
-	}
+	const bool terrain_source_valid = (valid_rng_terrain || valid_opt_flow_terrain);
+
+	_terrain_valid = terrain_source_valid && positive_hagl && small_relative_hagl_var;
 }

src/modules/ekf2/EKF2.cpp

@@ -159,10 +159,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_noise(_params->ekf2_rng_noise),
 	_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
 	_param_ekf2_rng_gate(_params->ekf2_rng_gate),
-	_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
 	_param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax),
 	_param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax),
-	_param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t),
 	_param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate),
 	_param_ekf2_rng_fog(_params->ekf2_rng_fog),
 	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
@@ -1619,6 +1617,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
+	// TODO: review -- we should merge getHagl() and isTerrainEstimateValid() since they must always be used together
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
@@ -2399,15 +2398,14 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 
 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;
 
-			estimator::sensor::rangeSample range_sample {
+			estimator::rangeSample range_sample {
 				.time_us = optical_flow.timestamp_sample,
-				.rng = optical_flow.distance_m,
+				.range = optical_flow.distance_m,
 				.quality = quality,
+				.min_distance = optical_flow.min_ground_distance,
+				.max_distance = optical_flow.max_ground_distance,
 			};
 			_ekf.setRangeData(range_sample);
-
-			// set sensor limits
-			_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
 		}
 
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -2574,15 +2572,15 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
 		// EKF range sample
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
-			estimator::sensor::rangeSample range_sample {
+			estimator::rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
-				.rng = distance_sensor.current_distance,
+				.range = distance_sensor.current_distance,
 				.quality = distance_sensor.signal_quality,
+				.min_distance = distance_sensor.min_distance,
+				.max_distance = distance_sensor.max_distance,
 			};
-			_ekf.setRangeData(range_sample);
 
-			// Save sensor limits reported by the rangefinder
-			_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
+			_ekf.setRangeData(range_sample);
 
 			_last_range_sensor_update = ekf2_timestamps.timestamp;
 		}

src/modules/ekf2/EKF2.hpp

@@ -353,7 +353,7 @@ private:
 	uint32_t _device_id_baro{0};
 	hrt_abstime _status_baro_hgt_pub_last{0};
 
-	float _last_baro_bias_published{};
+	float _last_baro_bias_published{NAN};
 
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 
@@ -618,10 +618,8 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
 		(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
 		(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
-		(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
 		(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
-		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
 		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,

src/modules/ekf2/params_range_finder.yaml

@@ -48,19 +48,10 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.1
+      default: 0.05
       min: 0.01
       unit: m
       decimal: 2
-    EKF2_RNG_PITCH:
-      description:
-        short: Range sensor pitch offset
-      type: float
-      default: 0.0
-      min: -0.75
-      max: 0.75
-      unit: rad
-      decimal: 3
     EKF2_RNG_A_VMAX:
       description:
         short: Maximum horizontal velocity allowed for conditional range aid mode
@@ -79,20 +70,10 @@ parameters:
           will not fuse range measurements to estimate its height. This only applies
           when conditional range aid mode is activated (EKF2_RNG_CTRL = 1).
       type: float
-      default: 5.0
+      default: 25.0
       min: 1.0
-      max: 10.0
+      max: 50.0
       unit: m
-    EKF2_RNG_QLTY_T:
-      description:
-        short: Minumum range validity period
-        long: Minimum duration during which the reported range finder signal quality
-          needs to be non-zero in order to be declared valid (s)
-      type: float
-      default: 1.0
-      unit: s
-      min: 0.1
-      max: 5
     EKF2_RNG_K_GATE:
       description:
         short: Gate size used for range finder kinematic consistency check
@@ -103,7 +84,7 @@ parameters:
           ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE)
           before tuning this gate.'
       type: float
-      default: 1.0
+      default: 0.5
       unit: SD
       min: 0.1
       max: 5.0
@@ -112,7 +93,7 @@ parameters:
         short: Range finder range dependent noise scaler
         long: Specifies the increase in range finder noise with range.
       type: float
-      default: 0.05
+      default: 0.01
       min: 0.0
       max: 0.2
       unit: m/m

src/modules/ekf2/params_terrain.yaml

@@ -28,7 +28,7 @@ parameters:
           where the range finder may be inside its minimum measurements distance when
           on ground.
       type: float
-      default: 0.1
-      min: 0.01
+      default: 0
+      min: 0
       unit: m
       decimal: 2

src/modules/ekf2/test/sensor_simulator/range_finder.cpp

@@ -17,7 +17,6 @@ void RangeFinder::send(uint64_t time)
 {
 	_range_sample.time_us = time;
 	_ekf->setRangeData(_range_sample);
-	_ekf->set_rangefinder_limits(_min_distance, _max_distance);
 }
 
 void RangeFinder::setData(float range_data_meters, int8_t range_quality)

src/modules/ekf2/test/sensor_simulator/range_finder.h

@@ -55,7 +55,7 @@ public:
 	void setLimits(float min_distance_m, float max_distance_m);
 
 private:
-	estimator::sensor::rangeSample _range_sample{};
+	estimator::rangeSample _range_sample{};
 	float _min_distance{0.2f};
 	float _max_distance{20.0f};
 

src/modules/ekf2/test/test_SensorRangeFinder.cpp

@@ -34,10 +34,9 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/common.h"
-#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
 #include <matrix/math.hpp>
 
-using estimator::sensor::rangeSample;
+using estimator::rangeSample;
 using matrix::Dcmf;
 using matrix::Eulerf;
 using namespace estimator::sensor;
@@ -48,10 +47,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_range_finder.setPitchOffset(0.f);
-		_range_finder.setCosMaxTilt(0.707f);
 		_range_finder.setLimits(_min_range, _max_range);
-		_range_finder.setMaxFogDistance(2.f);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test

src/modules/logger/logged_topics.cpp

@@ -372,6 +372,7 @@ void LoggedTopics::add_high_rate_sensors_topics()
 	add_topic_multi("sensor_optical_flow", 0, 2);
 	add_topic_multi("sensor_gps", 0, 4);
 	add_topic_multi("sensor_mag", 0, 4);
+	add_topic_multi("sensor_baro", 0, 4);
 }
 
 void LoggedTopics::add_mavlink_tunnel()

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -814,7 +814,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg)
 	report.max_distance = static_cast<float>(msg.range_max());
 	report.current_distance = static_cast<float>(msg.ranges()[0]);
 	report.variance = 0.0f;
-	report.signal_quality = -1;
+	report.signal_quality = 100;
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 
 	gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation();