diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg
index 8ab5b3d3e4b6..7fed72cc2814 100644
--- a/msg/EstimatorStatusFlags.msg
+++ b/msg/EstimatorStatusFlags.msg
@@ -49,6 +49,7 @@ bool cs_valid_fake_pos          # 41 - true if a valid constant position is bein
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 bool cs_gnss_vel                # 44 - true if GNSS velocity measurement fusion is intended
+bool cs_rng_kin_unknown      	# 45 - true when the range finder kinematic consistency check is not running
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
index e6b520b60024..d6fa31dcb884 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
@@ -36,56 +36,128 @@
  */
 
 #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;
-	}
+	_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);
+	_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
+	// prediction step
+	_time_last_update_us = time_us;
+	_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;
 
-	// 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;
+	// iterate through both measurements (z and dist_bottom)
+	const Vector2f measurements{z, dist_bottom};
 
-	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);
+	for (int measurement_idx = 0; measurement_idx < 2; measurement_idx++) {
 
-	updateConsistency(vz, time_us);
+		// dist_bottom
+		Vector2f H = _Ht;
+		float R = dist_bottom_var;
 
-	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
+		// z, direct state measurement
+		if (measurement_idx == 0) {
+			H(RangeFilter::z.idx) = 1.f;
+			H(RangeFilter::terrain.idx) = 0.f;
+			R = z_var;
+
+		}
+
+		// 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);
+		}
+
+		// 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::updateConsistency(float vz, uint64_t time_us)
+void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var)
 {
-	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;
+	// start the consistency check after 1s
+	if (_t_since_first_sample > _t_to_init) {
+		if (fabsf(_test_ratio_lpf.getState()) < 1.f) {
+			const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f);
+
+			if (!horizontal_motion && vertical_motion) {
+				_state = KinematicState::CONSISTENT;
+
+			} else if (_state == KinematicState::CONSISTENT || vertical_motion) {
+				_state = KinematicState::UNKNOWN;
+			}
+
+		} else {
+			_t_since_first_sample = 0.f;
+			_state = KinematicState::INCONSISTENT;
 		}
 
 	} 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;
-		}
+		_t_since_first_sample += dt;
 	}
 }
+
+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) {
+		_last_posD_reset_count = current_posD_reset_count;
+		_initialized = false;
+	}
+
+	update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
+}
diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
index d031e12d975b..0fb9e11a5e38 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
@@ -48,36 +48,62 @@ class RangeFinderConsistencyCheck final
 	RangeFinderConsistencyCheck() = default;
 	~RangeFinderConsistencyCheck() = default;
 
-	void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us);
+	enum class KinematicState : int {
+		INCONSISTENT = 0,
+		CONSISTENT = 1,
+		UNKNOWN = 2
+	};
 
-	void setGate(float gate) { _gate = gate; }
+	float getTestRatioLpf() const { return _initialized ? _test_ratio_lpf.getState() : 0.f; }
+	float getInnov() const { return _initialized ? _innov : 0.f; }
+	float getInnovVar() const { return _initialized ? _innov_var : 0.f; }
+	bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT && _t_since_first_sample > _t_to_init; }
+	bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT && _t_since_first_sample > _t_to_init; }
+	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()
+	{
+		if (_initialized && _state == KinematicState::CONSISTENT) {
+			_state = KinematicState::UNKNOWN;
+		}
 
-	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; }
+		_initialized = false;
+	}
 
-private:
-	void updateConsistency(float vz, uint64_t time_us);
-
-	uint64_t _time_last_update_us{};
-	float _dist_bottom_prev{};
-
-	float _test_ratio{};
-	AlphaFilter<float> _signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
-	float _gate{.2f};
-	float _innov{};
-	float _innov_var{};
-
-	bool _is_kinematically_consistent{true};
-	uint64_t _time_last_inconsistent_us{};
-	uint64_t _time_last_horizontal_motion{};
+	uint8_t current_posD_reset_count{0};
+	bool horizontal_motion{false};
 
-	static constexpr float _signed_test_ratio_tau = 2.f;
+private:
 
-	static constexpr float _min_vz_for_valid_consistency = .5f;
-	static constexpr uint64_t _consistency_hyst_time_us = 1e6;
+	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 _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
diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
index e6cd566a3fef..422c594e29b9 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
@@ -51,45 +51,49 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		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.rng_sens_pitch);
-		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
-		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
-		_range_sensor.setMaxFogDistance(_params.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.range_noise_scaler * _range_sensor.getDistBottom());
-				const float var = sq(_params.range_noise) + dist_dependant_var;
-
-				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
-			}
+		if (_range_sensor.isDataReady()) {
 
-		} 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.setPitchOffset(_params.rng_sens_pitch);
+			_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+			_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+			_range_sensor.setMaxFogDistance(_params.rng_fog);
+			_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
+
+			_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
+
+			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());
+
+				const float dist_var = getRngVar();
+				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
+
+				const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f);
 
+				if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) {
+					_rng_consistency_check.reset();
+				}
+
+				_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
+				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);
+				_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
+							   dist_var, imu_sample.time_us);
+
+			} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
 				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setValidity(true); // bypass the checks
+				_range_sensor.setValidity(true);
+
+			} else {
+				_rng_consistency_check.reset();
 			}
 		}
 
 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
+		_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
+							&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
 	} else {
 		return;
@@ -97,7 +101,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 
 	auto &aid_src = _aid_src_rng_hgt;
 
-	if (rng_data_ready && _range_sensor.getSampleAddress()) {
+	if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {
 
 		updateRangeHagl(aid_src);
 		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -107,13 +111,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				&& _control_status.flags.tilt_align
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy()
-				&& _rng_consistency_check.isKinematicallyConsistent();
+				&& _rng_consistency_check.isNotKinematicallyInconsistent();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
-
 		const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
 						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
 						      && isConditionalRangeAidSuitable();
@@ -137,7 +140,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -163,7 +167,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
 					_control_status.flags.rng_hgt = true;
 
-					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
+					if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
+					    && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -200,7 +205,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 						stopRngHgtFusion();
 						stopRngTerrFusion();
 
-					} else {
+					} else if (_rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -221,7 +226,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 					}
 
 				} else {
-					if (aid_src.innovation_rejected) {
+					if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
 						resetTerrainToRng(aid_src);
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}
@@ -268,11 +273,9 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.range_noise)
-		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
-		       0.f);
+	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+	const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+	return dist_var;
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
index ab1c747e017d..891447a4cd93 100644
--- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
@@ -164,6 +164,9 @@ void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t t
 		}
 
 		_prev_median_dist = median_dist;
+
+	} else {
+		_prev_median_dist = dist_bottom;
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 5ec5e429254d..a6a6ee6b5622 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -422,7 +422,7 @@ struct parameters {
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
 	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
-	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
+	float range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
 	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
@@ -622,6 +622,7 @@ uint64_t mag_heading_consistent  :
 		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 		uint64_t baro_fault              : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used
 		uint64_t gnss_vel                : 1; ///< 44 - true if GNSS velocity measurement fusion is intended
+		uint64_t rng_kin_unknown	 : 1; ///< 45 - true when the range finder kinematic consistency check is not running
 	} flags;
 	uint64_t value;
 };
diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp
index e7b2eea396b1..3a8ba4b2440c 100644
--- a/src/modules/ekf2/EKF/covariance.cpp
+++ b/src/modules/ekf2/EKF/covariance.cpp
@@ -227,6 +227,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (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
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 1dad06d3042a..3bab2cd8bc08 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -116,7 +116,7 @@ class Ekf final : public EstimatorInterface
 
 	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)
diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
index 91c3fd734ea8..7c8baf1fecee 100755
--- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
+++ b/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 @@ def compute_gravity_z_innov_var_and_h(
 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)
diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_P_init.h
new file mode 100644
index 000000000000..b7e4e62e73f1
--- /dev/null
+++ b/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
diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/range_validation_filter_h.h
new file mode 100644
index 000000000000..91deb51ed956
--- /dev/null
+++ b/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
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 974e748719d3..f39501ee6ade 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -1936,6 +1936,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 		status_flags.cs_baro_fault	    = _ekf.control_status_flags().baro_fault;
 		status_flags.cs_gnss_vel            = _ekf.control_status_flags().gnss_vel;
+		status_flags.cs_rng_kin_unknown     = _ekf.control_status_flags().rng_kin_unknown;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml
index 97f60bdff4c2..90525178af5f 100644
--- a/src/modules/ekf2/params_range_finder.yaml
+++ b/src/modules/ekf2/params_range_finder.yaml
@@ -113,7 +113,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
diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp
index 255f6597b3f5..8b5d02f2aab7 100644
--- a/src/modules/ekf2/test/test_EKF_accelerometer.cpp
+++ b/src/modules/ekf2/test/test_EKF_accelerometer.cpp
@@ -192,12 +192,13 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange)
 	_sensor_simulator.runSeconds(5);
 
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND: an accelerometer with a really large Z bias
 	const float bias = CONSTANTS_ONE_G;
 	_sensor_simulator._imu.setAccelData(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G + bias));
 	_sensor_simulator.runSeconds(2);
+	_sensor_simulator._imu.setAccelClipping(false, false, true);
+	_sensor_simulator.runSeconds(2);
 
 	// THEN: the bad vertical is detected because both sources agree
 	EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp
index 7e0070397168..1bf1fd784997 100644
--- a/src/modules/ekf2/test/test_EKF_flow.cpp
+++ b/src/modules/ekf2/test/test_EKF_flow.cpp
@@ -171,7 +171,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround)
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 	_sensor_simulator.startRangeFinder();
-	_sensor_simulator.runSeconds(1.0);
+	_sensor_simulator.runSeconds(2.0);
 
 	// THEN: estimated velocity should match simulated velocity
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp
index 3b9cd2363b7b..2440bbe11fdc 100644
--- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp
+++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp
@@ -103,7 +103,7 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 	/* _ekf_wrapper.enableExternalVisionHeightFusion(); */ //TODO: this currently sets the reference to EV
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -125,9 +125,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus();
 	EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f);
 
-	const float terrain = _ekf->getTerrainVertPos();
-	EXPECT_NEAR(terrain, -baro_increment, 1.2f);
-
 	const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus();
 	EXPECT_EQ(ev_status.bias, 0.f);
 
@@ -139,7 +136,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion());
 
 	// AND WHEN: the gps height increases
 	const float gps_increment = 1.f;
@@ -150,9 +146,6 @@ TEST_F(EkfHeightFusionTest, baroRef)
 	EXPECT_EQ(gps_status.bias, _ekf->getGpsHgtBiasEstimatorStatus().bias);
 	// the estimated height follows the GPS height
 	EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f);
-	// and the range finder bias is adjusted to follow the new reference
-	const float terrain2 = _ekf->getTerrainVertPos();
-	EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f);
 }
 
 TEST_F(EkfHeightFusionTest, gpsRef)
@@ -162,7 +155,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -174,7 +167,7 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 
 	const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus();
 	const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt;
-	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f);
+	EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.8f);
 
 	// AND WHEN: the baro data increases
 	const float baro_increment = 5.f;
@@ -215,7 +208,7 @@ TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion)
 	_ekf_wrapper.enableBaroHeightFusion();
 	_ekf_wrapper.disableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
-	_sensor_simulator.runSeconds(1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -242,7 +235,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	_ekf_wrapper.enableGpsHeightFusion();
 	_ekf_wrapper.enableRangeHeightFusion();
 
-	_sensor_simulator.runSeconds(0.1);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO);
 
@@ -251,7 +244,7 @@ TEST_F(EkfHeightFusionTest, baroRefFailOver)
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS);
 
 	_sensor_simulator.stopGps();
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(14);
 	EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE);
 
 	_sensor_simulator.stopRangeFinder();
@@ -408,7 +401,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
 	reset_logging_checker.capturePostResetState();
 
 	// An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS)
-	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f);
+	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.4f);
 
 	EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f);
 
diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp
index 7fbf7fcd3b64..ba50c790ae65 100644
--- a/src/modules/ekf2/test/test_EKF_terrain.cpp
+++ b/src/modules/ekf2/test/test_EKF_terrain.cpp
@@ -207,7 +207,7 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(10);
+	_sensor_simulator.runSeconds(100);
 
 	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
@@ -219,11 +219,36 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 {
 	// GIVEN: rng for terrain but not flow
 	_ekf_wrapper.disableFlowFusion();
-	_ekf_wrapper.enableRangeHeightFusion();
-
-	const float rng_height = 18;
-	const float flow_height = 1.f;
-	runFlowAndRngScenario(rng_height, flow_height);
+	const float rng_height = 16.f;
+
+	_sensor_simulator.startGps();
+	_ekf->set_min_required_gps_health_time(1e6);
+	_ekf->set_in_air_status(false);
+	_ekf->set_vehicle_at_rest(true);
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+	const Vector3f simulated_velocity(0.5f, -1.0f, 0.0f);
+
+	// Configure GPS simulator data
+	_sensor_simulator._gps.setVelocity(simulated_velocity);
+	_sensor_simulator._gps.setPositionRateNED(simulated_velocity);
+
+	// Simulate flight above max range finder distance
+	// run for a while to let terrain uncertainty increase
+	_sensor_simulator._rng.setData(30.f, 100);
+	_sensor_simulator._rng.setLimits(0.1f, 20.f);
+	_sensor_simulator.startRangeFinder();
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_sensor_simulator.runSeconds(40);
+
+	// quick range finder change to bypass stuck-check
+	_sensor_simulator._rng.setData(rng_height - 1.f, 100);
+	_sensor_simulator.runSeconds(1);
+	_sensor_simulator._rng.setData(rng_height, 100);
+	_sensor_simulator.runSeconds(10);
 
 	// THEN: the terrain should reset using rng
 	EXPECT_NEAR(rng_height, _ekf->getHagl(), 1e-3f);
@@ -234,13 +259,13 @@ TEST_F(EkfTerrainTest, testRngStartInAir)
 
 	const float corr_terrain_vz = P(State::vel.idx + 2,
 					State::terrain.idx) / sqrtf(_ekf->getVelocityVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_vz, 0.6f, 0.03f);
+	EXPECT_TRUE(fabsf(corr_terrain_vz) > 0.6f && fabsf(corr_terrain_vz) < 1.f);
 
 	const float corr_terrain_z = P(State::pos.idx + 2,
 				       State::terrain.idx) / sqrtf(_ekf->getPositionVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_z, 0.8f, 0.03f);
+	EXPECT_TRUE(fabsf(corr_terrain_z) > 0.8f && fabsf(corr_terrain_z) < 1.f);
 
 	const float corr_terrain_abias_z = P(State::accel_bias.idx + 2,
 					     State::terrain.idx) / sqrtf(_ekf->getAccelBiasVariance()(2) * var_terrain);
-	EXPECT_NEAR(corr_terrain_abias_z, -0.4f, 0.03f);
+	EXPECT_TRUE(corr_terrain_abias_z < -0.1f);
 }