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 e6b520b600..fed9170a7a 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
@@ -37,55 +37,63 @@
 
 #include <aid_sources/range_finder/range_finder_consistency_check.hpp>
 
-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
-		bool horizontal_motion, uint64_t time_us)
-{
-	if (horizontal_motion) {
-		_time_last_horizontal_motion = time_us;
-	}
+using namespace matrix;
 
+void RangeFinderConsistencyCheck::initMiniKF(float p1, float p2, float x1, float x2)
+{
+	_R.setZero();
+	_A.setIdentity();
+	float p[4] = {p1, 0.f, 0.f, p2};
+	_P = Matrix<float, 2, 2>(p);
+	float h[4] = {1.f, 0.f, -1.f, 1.f};
+	_H = Matrix<float, 2, 2>(h);
+	_x(0) = x1;
+	_x(1) = x2;
+	_initialized = true;
+	_sample_count = 0;
+}
+
+void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom,
+		float dist_bottom_var, uint64_t time_us)
+{
 	const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
 
-	if ((_time_last_update_us == 0)
-	    || (dt < 0.001f) || (dt > 0.5f)) {
+	if (_time_last_update_us == 0 || dt > 1.f) {
 		_time_last_update_us = time_us;
-		_dist_bottom_prev = dist_bottom;
 		return;
 	}
 
-	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
-	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down
-
-	// 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);
-
 	_time_last_update_us = time_us;
-	_dist_bottom_prev = dist_bottom;
-}
 
-void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
-{
-	if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) {
-		if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) {
-			_is_kinematically_consistent = false;
-			_time_last_inconsistent_us = time_us;
-		}
+	_R(0, 0) = z_var;
+	_R(1, 1) = dist_bottom_var;
+
+	SquareMatrix<float, 2> Q;
+	const float process_noise = 0.01f;
+	Q(0, 0) = dt * dt * vz_var + process_noise;
+	Q(1, 1) = process_noise * 0.5f;
+	_x(0) += dt * vz;
+	_P = _A * _P * _A.transpose() + Q;
+
+	const Vector2f measurements(z, dist_bottom);
+	const Vector2f y = measurements - _H * _x;
+	const Matrix2f S = _H * _P * _H.transpose() + _R;
+	float test_ratio = math::min(sq(y(1)) / (sq(_gate) * S(1, 1)), 2.f);
+
+	if (test_ratio < 1.f) {
+		Matrix2f K = _P * _H.transpose() * S.I();
+		_x = _x + K * y;
+		_P = _P - K * _H * _P;
+		_P = 0.5f * (_P + _P.transpose()); // Ensure symmetry
+	}
+
+	_innov = y(1);
+	_innov_var = S(1, 1);
+
+	if (_sample_count++ > 10) {
+		_is_kinematically_consistent  = _test_ratio_lpf.update(test_ratio) < 1.f;
 
 	} 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;
-		}
+		_test_ratio_lpf.update(test_ratio);
 	}
 }
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 d031e12d97..3036f4ed2b 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,33 @@ 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);
-
-	void setGate(float gate) { _gate = gate; }
-
-	float getTestRatio() const { return _test_ratio; }
-	float getSignedTestRatioLpf() const { return _signed_test_ratio_lpf.getState(); }
+	float getTestRatioLpf() const { return _test_ratio_lpf.getState(); }
 	float getInnov() const { return _innov; }
 	float getInnovVar() const { return _innov_var; }
+
 	bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
+	void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
+			  uint64_t time_us);
+	void initMiniKF(float p1, float p2, float x1, float x2);
+	void stopMiniKF() { _initialized = false; }
+	bool isRunning() { return _initialized; }
+
+	matrix::SquareMatrix<float, 2> _R{};
+	matrix::SquareMatrix<float, 2> _P{};
+	matrix::SquareMatrix<float, 2> _A{};
+	matrix::SquareMatrix<float, 2> _H{};
+	matrix::Vector2f _x{};
+	bool _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{};
-
+	uint64_t _time_last_update_us{0};
+	float _dist_bottom_prev{};
+	AlphaFilter<float> _test_ratio_lpf{0.3}; // average signed test ratio used to detect a bias in the data
+	float _gate{1.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;
+	int _sample_count{0};
 };
 
 #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 35d56422f2..9b19e1698d 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
@@ -68,13 +68,24 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 			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 bool vertical_motion = sq(_state.vel(2)) > fmaxf(P(State::vel.idx + 2, State::vel.idx + 2), 0.1f);
 
-				const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom());
-				const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var;
+				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+				const float dist_var = sq(_params.range_noise) + dist_dependant_var;
+
+				if (horizontal_motion) {
+					_rng_consistency_check.stopMiniKF();
+
+				} else if (vertical_motion) {
+					if (!_rng_consistency_check.isRunning()) {
+						_rng_consistency_check.initMiniKF(P(State::pos.idx + 2, State::pos.idx + 2), P(State::terrain.idx, State::terrain.idx),
+										  _state.pos(2), _state.terrain);
+					}
+
+					_rng_consistency_check.UpdateMiniKF(_state.pos(2), P(State::pos.idx + 2, State::pos.idx + 2), _state.vel(2),
+									    P(State::vel.idx + 2, State::vel.idx + 2), _range_sensor.getRange(), dist_var, imu_sample.time_us);
+				}
 
-				_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
 			}
 
 		} else {
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index c4aaed1cbf..bd4412d92d 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/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)