port over relevant code from other branch

notes.md

@@ -0,0 +1,63 @@
+# Rangefinder in PX4
+The rangefinder distance (Range) can be used for 2 distinct purposes: EKF Z Position/Velocity observation, and Terrain Height observation.
+
+#### Definitions
+- Altitude = EKF Z Position
+- Range = Distance to ground (kinematically consistent, gated)
+- Terrain Height = Altitude - Range
+- Kinematic Consisteny (KC) = Measurements are consistent with aircrafts estimated motion.
+
+## Use cases
+- Improve Altitude Hold stabilization while hovering. Mitigate disturbances from wind gusts, GPS multipath, and vibration.
+- Fixed wing landing. Terrain Height is important for glide slope.
+- Multi-Copter takeoff height accuracy. GPS accuracy is poor while on the ground and barometer suffers from ground effect.
+- Multi-Copter landing. Same as above.
+- Optical Flow. Velocity estimate relies on Range to convert from Pixel_Vx/Vy to Body_Vx/Vy.
+- Anything else?
+
+---
+
+# Discussion
+
+## Problems
+Currently the rangefinder usage in PX4 has many bugs and is generally broken. The KC check uses an LPF with a slow time constant and requires vertical motion. The stuck and fog checks can cause false positives due to the simple logic. The terrain estimate doesn't reflect reality. The baro ground effect cause the dist_bottom to grow during takeoff and therefore corrupt the estimate.
+
+## How it should work
+The rangefinder distance to ground should be fused into the Altitude Estimate while on the ground and during takeoff. If the measurements are valid and consistent, they represent the most accurate distance to ground. If a takeoff to 2.5m is issued, the local position setpoint, position, and dist_bottom should all be 2.5m.
+
+There is (almost?) no use case for Terrain Following. I think most users would expect the drone to hold absolute position in space if an absolute reference is available (GPS, Vision, Baro relative to Home). The default behavior should be fuse Range measurements as the primary Altitude reference as long as the samples are KC. This ensures that the drone takes off to the correct altitude. Since baro suffers from noise (wind gusts) the fusion of high precision rangefinder measurements while KC should allow minimal deviation of the Local Z Position. After Range fusion is disabled once due to KC check failure, the Local Positon Z can deviate from the Range. This is expected as Terrain can change. During hover while the Range is KC, the measurements are fused and provide a highly accurate observation to ensure hover altitude is maintained precisely.
+
+Indoors flight is also worth considering. Currently users must change EKF2_HGT_REF to Range in order to fly indoors. I think we could be smarter about this and detect when the baro is unreliable and switch to Range as primary altitude reference. We should see KC between Range and IMU_z samples and mark the baro as faulty. This can work during takeoff if Range is the primary reference from takeoff, we would see KC between Range and IMU and see a high baro test ratio.
+
+---
+
+# EKF Implementation
+
+### Raw Sample Pre-filtering
+Samples from the rangefinder are popped from a buffer and time-aligned with the IMU sample. The samples are rejected if the quality, tilt, or range are out of bounds. These are hard constraints.
+- quality <= 0
+- tilt > tilt_max
+- sample_rng > rng_max
+
+If samples are rejected or missing for long enough, a timeout occurs which disables fusion of Range into Altitude/Terrain Estimates.
+
+### Kinematic Consistency Check
+Samples are now considered potentially valid, but must pass through another filter which evaluates the current samples kinematic consistency. Range Fusion into Altitude/Terrain is disabled as soon as the measurements fail this check.
+
+### Fusion for Terrain Estimate
+- Must be KC (only requirement)
+	- If not KC, disable fusion
+
+**KC valid**
+- If fusion is disabled, reset the terrain estimate to range and zero aid src innovation.
+- Fuse measurement into Terrain state
+
+### Fusion for Altitude Estimate
+- Must be KC and RangeAidConditionsPassed (RangeAidConditionsPassed = (time gated, in_range, xy_vel < thresh))
+	- If not KC or RangeAidConditionsPassed, disable fusion
+
+**KC valid**
+- If fusion is disabled do nothing
+	- unless on ground, then reset altitude to zero
+	- unless innovation rejected, then zero aid src innovation.
+- Fuse measurement into Altitude state

range_alt_notes.txt

@@ -0,0 +1,20 @@
+## FlightTaskManualAltitude
+- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
+- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
+- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
+- _respectMinAltitude ... weird, remove? No need for a function
+
+
+## FlightTask
+- _dist_to_bottom and _dist_to_ground ... confusing, unify
+-
+
+## FlightTaskAuto
+- reuse logic for range alt lock
+- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
+
+## EKF
+- Vz does not reflect true Vz due to noisy baro
+- Vz errors cause rangefinder kinematic consistency check to fail
+- Position setpoint error remains over long periods (might be related to Vz issues above)
+

src/modules/ekf2/CMakeLists.txt

@@ -211,7 +211,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/jake_range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
@@ -241,7 +241,7 @@ px4_add_module(
 	COMPILE_FLAGS
 		${MAX_CUSTOM_OPT_LEVEL}
 		-fno-associative-math
-		#-DDEBUG_BUILD
+		-DDEBUG_BUILD
 		#-O0
 	INCLUDES
 		EKF

src/modules/ekf2/EKF/CMakeLists.txt

@@ -124,7 +124,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/jake_range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -158,8 +158,11 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
+
+		// DEBUGGING: disable flow fusion for terrain estimation to limit variables
+		_control_status.flags.opt_flow_terrain = false;
 		// If the height is relative to the ground, terrain height cannot be observed.
-		_control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
+		// _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
 
 		if (_control_status.flags.opt_flow) {
 			if (continuing_conditions_passing) {
@@ -189,7 +192,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		} else {
 			if (starting_conditions_passing) {
 				// If the height is relative to the ground, terrain height cannot be observed.
-				_control_status.flags.opt_flow_terrain = (_height_sensor_ref != HeightSensor::RANGE);
+				// DEBUGGING: disable flow fusion for terrain estimation to limit variables
+				// _control_status.flags.opt_flow_terrain = (_height_sensor_ref != HeightSensor::RANGE);
 
 				if (isHorizontalAidingActive()) {
 					if (fuseOptFlow(H, _control_status.flags.opt_flow_terrain)) {
@@ -215,7 +219,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					}
 				}
 
-				_control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
+				// DEBUGGING: disable flow fusion for terrain estimation to limit variables
+				// _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
 			}
 		}
 

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/jake_range_height_control.cpp

@@ -0,0 +1,411 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 range_height_control.cpp
+ * Control functions for ekf range finder height fusion
+ */
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"
+
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
+{
+	// Check if rangefinder is available/enabled
+	if (!_range_buffer) {
+		// ECL_INFO("no buff");
+		return;
+	}
+
+	// Pop rangefinder measurement from buffer of samples into active sample
+	sensor::rangeSample sample = {};
+	if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				stopRangeAltitudeFusion("sensor timed out");
+				stopRangeTerrainFusion("sensor timed out");
+			}
+			// ECL_INFO("timed out1");
+		}
+		return;
+	}
+
+	// Set the raw sample
+	_range_sensor.setSample(sample);
+
+	// TODO: move setting params to init function
+	// Set all of the parameters
+	_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+
+	// Update sensor to earth rotation
+	_range_sensor.updateSensorToEarthRotation(_R_to_earth);
+
+
+	// TODO: refactor into validity_checks()
+	// Gate sample consumption on these checks
+	bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
+	bool tilt_ok = _range_sensor.isTiltOk();
+	bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
+	// - Not stuck value
+	// - Not fog detected
+
+	// If quality, tilt, or value are outside of bounds -- throw away measurement
+	if (!quality_ok || !tilt_ok || !range_ok) {
+		if (_range_sensor.timedOut(imu_sample.time_us)) {
+			// Disable fusion if it's currently enabled
+			if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
+				const char* reason = "sensor data invalid";
+				stopRangeAltitudeFusion(reason);
+				stopRangeTerrainFusion(reason);
+			}
+			ECL_INFO("timed out2");
+		}
+
+		if (!quality_ok) {
+			ECL_INFO("!quality_ok");
+		}
+		if (!tilt_ok) {
+			ECL_INFO("!tilt_ok");
+		}
+		if (!range_ok) {
+			ECL_INFO("!range_ok");
+		} // commander takeoff
+		return;
+	}
+
+
+	// TODO: refactor into apply_body_offset()
+	// Correct the range data for position offset relative to the IMU
+	const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
+	const Vector3f imu_pos_body = _params.imu_pos_body;
+	const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
+	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+	sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
+
+	// Provide sample from buffer to object
+	_range_sensor.setSample(sample);
+
+
+	// 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();
+
+	const float z = _gpos.altitude();
+	const float vz = _state.vel(2);
+	const float dist = _range_sensor.getDistBottom(); // NOTE: applies rotation into world frame
+	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);
+
+	if (!PX4_ISFINITE(_aid_src_rng_hgt.observation) || !PX4_ISFINITE(_aid_src_rng_hgt.observation_variance)) {
+		ECL_INFO("INFINIFY OBSERVATION INVALID");
+	}
+
+	// Fuse Range data as Primary height source
+	// NOTE: terrain is not estimated in this mode
+	if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+		fuseRangeAsHeightSource();
+	} else {
+		// Fuse Range data as aiding height source (Primary GPS or Baro)
+		fuseRangeAsHeightAiding();
+	}
+}
+
+void Ekf::fuseRangeAsHeightAiding()
+{
+	// ISSUES: 6/25/2025
+	// - optical flow terrain fucks everything up, I've hardcoded disabled it
+	// - when rng fusion starts again, EKF Z state is corrupted (oscillation)
+	// -
+
+	const char* kNotKinematicallyConsistentText = "not kinematically consistent";
+	const char* kConditionsFailingText = "conditions failing";
+
+	// Stop fusion if rangefinder kinematic consistency fails
+	if (!_control_status.flags.rng_kin_consistent) {
+		stopRangeTerrainFusion(kNotKinematicallyConsistentText);
+		stopRangeAltitudeFusion(kNotKinematicallyConsistentText);
+		return;
+	}
+
+
+	//// TERRAIN FUSION ////
+
+	// Fuse Range into Terrain if:
+	// - kinematically consistent (hagl_rate < 1)
+
+	// Start fusion
+	if (!_control_status.flags.rng_terrain) {
+		ECL_INFO("START RNG Terrain fusion");
+		_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
+		ECL_INFO("resetting terrain to range");
+		resetTerrainToRng(_aid_src_rng_hgt);
+		resetAidSourceStatusZeroInnovation(_aid_src_rng_hgt);
+	}
+
+
+	//// ALTITUDE FUSION ////
+
+	bool range_aid_conditional = _params.rng_ctrl == RngCtrl::CONDITIONAL;
+	bool range_aid_enabled = _params.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) {
+			ECL_INFO("START RNG Altitude fusion");
+			_control_status.flags.rng_hgt = true;
+
+			// Reset altitude to rangefinder if on ground
+			if (!_control_status.flags.in_air) {
+				ECL_INFO("JAKE GND resetting altitude to range");
+				resetAltitudeTo(_aid_src_rng_hgt.observation - _state.terrain);
+			}
+
+			// TODO: review for correctness
+			if (_aid_src_rng_hgt.innovation_rejected) {
+				// Reset aid source
+				ECL_INFO("resetting aid source");
+				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);
+}
+
+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");
+		ECL_INFO("initializing range as primary");
+		_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)) {
+		ECL_INFO("RNG height fusion reset required, all height sources failing");
+
+		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()) {
+			ECL_INFO("resetting vertical velocity");
+			resetVerticalVelocityToZero();
+		}
+
+		_aid_src_rng_hgt.time_last_fuse = timestamp;
+	}
+}
+
+bool Ekf::rangeAidConditionsPassed()
+{
+	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
+			ECL_INFO("RNG AID conditions valid");
+			_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) {
+			// 	ECL_INFO("!is_hagl_stable");
+			// }
+			// if (is_horizontal_aiding_active && !is_below_max_speed) {
+			if (!is_below_max_speed) {
+				ECL_INFO("!is_below_max_speed");
+			}
+			if (!is_in_range) {
+				ECL_INFO("!is_in_range");
+			}
+		}
+	}
+
+	return conditions_passing;
+}
+
+void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
+{
+	// Since the distance is not a direct observation of the terrain state but is based
+	// on the height state, a reset should consider the height uncertainty. This can be
+	// done by manipulating the Kalman gain to inject all the innovation in the terrain state
+	// and create the correct correlation with the terrain state with a covariance update.
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
+
+	const float old_terrain = _state.terrain;
+
+	VectorState H;
+	sym::ComputeHaglH(&H);
+
+	VectorState K;
+	K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+
+	// record the state change
+	const float delta_terrain = _state.terrain - old_terrain;
+
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+{
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
+	const float measurement_variance = getRngVar();
+
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.sample()->time_us, // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate
+
+	// 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;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
+}
+
+void Ekf::stopRangeAltitudeFusion(const char* reason)
+{
+	if (_control_status.flags.rng_hgt) {
+		ECL_INFO("STOP RNG Altitude fusion: %s", reason);
+		_control_status.flags.rng_hgt = false;
+		if (_height_sensor_ref == HeightSensor::RANGE) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
+	}
+}
+
+void Ekf::stopRangeTerrainFusion(const char* reason)
+{
+	if (_control_status.flags.rng_terrain) {
+		ECL_INFO("STOP RNG Terrain fusion: %s", reason);
+		_control_status.flags.rng_terrain = false;
+	}
+}

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

@@ -36,56 +36,142 @@
  */
 
 #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_lpf.reset(0.f);
+	_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_lpf.setParameters(dt, _t_to_init);
+			// _test_ratio_lpf.update(sign(_innov) * test_ratio);
+			_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)
+{
+	// start the consistency check after 1s
+	// if (_t_since_first_sample < _t_to_init) {
+	// 	_t_since_first_sample += dt;
+	// 	return;
+	// }
+
+	// TODO: magic test ratio
+	// if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
+	if (fabsf(_test_ratio) > 1.f) {
+		// printf("KinematicState::INCONSISTENT\n");
+		_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

@@ -48,36 +48,58 @@ 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 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};
+	// AlphaFilter<float> _test_ratio_lpf{time_constant};
 
-	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/sensor_range_finder.cpp

@@ -47,10 +47,34 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
+void SensorRangeFinder::setSample(const rangeSample &sample)
 {
-	updateSensorToEarthRotation(R_to_earth);
-	updateValidity(current_time_us);
+	_sample = sample;
+}
+
+bool SensorRangeFinder::timedOut(uint64_t time_now) const
+{
+	if (_sample.time_us > time_now) {
+		return false;
+	}
+
+	// TODO: 200ms?
+	return time_now > _sample.time_us + 200'000;
+}
+
+void SensorRangeFinder::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 SensorRangeFinder::setLimits(float min_distance, float max_distance)
+{
+	_rng_valid_min_val = min_distance;
+	_rng_valid_max_val = max_distance;
 }
 
 void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -60,112 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear
 	_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

@@ -41,8 +41,6 @@
 #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>
 
@@ -57,105 +55,62 @@ struct rangeSample {
 	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
+class SensorRangeFinder
 {
 public:
 	SensorRangeFinder() = default;
-	~SensorRangeFinder() override = default;
+	~SensorRangeFinder() = 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; }
+	struct Parameters {
+		float ekf2_imu_pos_x{};
+		float ekf2_imu_pos_y{};
+		float ekf2_imu_pos_z{};
+
+		float ekf2_rng_pos_x{};
+		float ekf2_rng_pos_y{};
+		float ekf2_rng_pos_z{};
+
+		float ekf2_rng_pitch{};
+
+		float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt
+	};
+
+	void updateParameters(Parameters& parameters) { _parameters = parameters; };
+
+	bool timedOut(uint64_t time_now) const;
+	void setSample(const rangeSample &sample);
 
-	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; }
+	rangeSample *sample() { 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 setPitchOffset(float 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; }
+	void setLimits(float min_distance, float max_distance);
+
+	// 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);
+
+private:
 
 	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)
+	Parameters _parameters{};
 
 	/*
 	 * Tilt check
@@ -172,20 +127,6 @@ private:
 	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

src/modules/ekf2/EKF/covariance.cpp

@@ -106,6 +106,7 @@ void Ekf::initialiseCovariance()
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// use the ground clearance value as our uncertainty
+	// TODO: ^ why?
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.ekf2_min_rng));
 #endif // CONFIG_EKF2_TERRAIN
 }
@@ -227,6 +228,11 @@ 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)
+		// TODO: explain
+		_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,18 @@ 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)
+
+	sensor::SensorRangeFinder::Parameters params = {};
+	// params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get();
+	// params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get();
+	// params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get();
+	// params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get();
+	// params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get();
+	// params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get();
+	params.ekf2_rng_pitch = _params.ekf2_rng_pitch;
+	_range_sensor.updateParameters(params);
+#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)
@@ -779,9 +779,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/estimator_interface.h

@@ -121,7 +121,6 @@ public:
 		_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)
@@ -376,6 +375,10 @@ protected:
 	RingBuffer<sensor::rangeSample> *_range_buffer {nullptr};
 	uint64_t _time_last_range_buffer_push{0};
 
+	// Range aiding
+	bool _rng_aid_conditions_valid{false};
+	uint64_t _time_rng_aid_conditions_valid{0};
+
 	sensor::SensorRangeFinder _range_sensor{};
 	RangeFinderConsistencyCheck _rng_consistency_check;
 #endif // CONFIG_EKF2_RANGE_FINDER

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

@@ -42,6 +42,7 @@
 
 void Ekf::initTerrain()
 {
+	// JAKE: review
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.ekf2_min_rng;
 
@@ -54,7 +55,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 +78,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 +92,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: quantigy 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

@@ -1619,6 +1619,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
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();

src/modules/ekf2/params_range_finder.yaml

@@ -48,7 +48,7 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.1
+      default: 0.05
       min: 0.01
       unit: m
       decimal: 2
@@ -79,20 +79,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 +93,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 +102,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/test_SensorRangeFinder.cpp

@@ -51,7 +51,6 @@ public:
 		_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/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();