diff --git a/EKF/CMakeLists.txt b/EKF/CMakeLists.txt
index 09c2a8b48b..b5548bc8ec 100644
--- a/EKF/CMakeLists.txt
+++ b/EKF/CMakeLists.txt
@@ -47,9 +47,9 @@ add_library(ecl_EKF
 	terrain_estimator.cpp
 	vel_pos_fusion.cpp
 	gps_yaw_fusion.cpp
-	range_finder_checks.cpp
 	imu_down_sampler.cpp
 	EKFGSF_yaw.cpp
+	sensor_range_finder.cpp
 )
 
 add_dependencies(ecl_EKF prebuild_targets)
diff --git a/EKF/Sensor.hpp b/EKF/Sensor.hpp
new file mode 100644
index 0000000000..0ff2e5cfad
--- /dev/null
+++ b/EKF/Sensor.hpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 Estimation and Control Library (ECL). 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 ECL 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>
+ *
+ */
+
+#pragma once
+
+#include "common.h"
+
+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;
+};
+
+} // namespace sensor
+} // namespace estimator
diff --git a/EKF/common.h b/EKF/common.h
index ceb538cdff..ce1743d38f 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -288,8 +288,6 @@ struct parameters {
 	int32_t range_aid{0};			///< allow switching primary height source to range finder if certain conditions are met
 	float range_aid_innov_gate{1.0f}; 	///< gate size used for innovation consistency checks for range aid fusion
 	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_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)
-	int32_t range_signal_hysteresis_ms{1000}; 	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (ms)
 
 	// vision position fusion
         float ev_vel_innov_gate{3.0f};		///< vision velocity fusion innovation consistency gate size (STD)
diff --git a/EKF/control.cpp b/EKF/control.cpp
index a93f6af46d..a581028879 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -116,20 +116,24 @@ void Ekf::controlFusionModes()
 		_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
 	}
 
-	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
-	// this is required for use of range finder and flow data
-	_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
 
+
+	{
 	// Get range data from buffer and check validity
-	_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed);
+	const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
+	_range_sensor.setDataReadiness(is_rng_data_ready);
+	_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
 
-	updateRangeDataValidity();
+	// update range sensor angle parameters in case they have changed
+	_range_sensor.setTiltOffset(_params.rng_sens_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	}
 
-	if (_range_data_ready && _rng_hgt_valid) {
+	if (_range_sensor.isDataHealthy()) {
 		// correct the range data for position offset relative to the IMU
 		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
 		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
-		_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
+		_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 	}
 
 	// We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon.
@@ -853,7 +857,7 @@ void Ekf::controlHeightSensorTimeouts()
 
 		} else if (_control_status.flags.rng_hgt) {
 
-			if (_rng_hgt_valid) {
+			if (_range_sensor.isHealthy()) {
 				request_height_reset = true;
 				ECL_WARN_TIMESTAMPED("rng hgt timeout - reset to rng hgt");
 
@@ -933,7 +937,7 @@ void Ekf::checkVerticalAccelerationHealth()
 void Ekf::controlHeightFusion()
 {
 	checkRangeAidSuitability();
-	_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
+	const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();
 
 	bool fuse_height = false;
 
@@ -943,22 +947,17 @@ void Ekf::controlHeightFusion()
 
 	// FALLTHROUGH
 	case VDIST_SENSOR_BARO:
-		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
+		if (do_range_aid && _range_sensor.isDataHealthy()) {
 			setControlRangeHeight();
 			fuse_height = true;
 
 			// we have just switched to using range finder, calculate height sensor offset such that current
 			// measurement matches our current height estimate
 			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
-				if (isTerrainEstimateValid()) {
-					_hgt_sensor_offset = _terrain_vpos;
-
-				} else {
-					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
-				}
+				_hgt_sensor_offset = _terrain_vpos;
 			}
 
-		} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
+		} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
 			setControlBaroHeight();
 			fuse_height = true;
 
@@ -990,9 +989,9 @@ void Ekf::controlHeightFusion()
 		break;
 
 	case VDIST_SENSOR_RANGE:
-		if (_range_data_ready && _rng_hgt_valid) {
+		if (_range_sensor.isDataHealthy()) {
 			setControlRangeHeight();
-			fuse_height = _range_data_ready;
+			fuse_height = true;
 
 			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
 				// we have just switched to using range finder, calculate height sensor offset such that current
@@ -1002,7 +1001,7 @@ void Ekf::controlHeightFusion()
 					_hgt_sensor_offset = _terrain_vpos;
 
 				} else if (_control_status.flags.in_air) {
-					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
+					_hgt_sensor_offset = _range_sensor.getCosTilt() * _range_sensor.getRange() + _state.pos(2);
 
 				} else {
 					_hgt_sensor_offset = _params.rng_gnd_clearance;
@@ -1025,22 +1024,17 @@ void Ekf::controlHeightFusion()
 	case VDIST_SENSOR_GPS:
 
 		// Determine if GPS should be used as the height source
-		if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
+		if (do_range_aid && _range_sensor.isDataHealthy()) {
 			setControlRangeHeight();
 			fuse_height = true;
 
 			// we have just switched to using range finder, calculate height sensor offset such that current
 			// measurement matches our current height estimate
 			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
-				if (isTerrainEstimateValid()) {
-					_hgt_sensor_offset = _terrain_vpos;
-
-				} else {
-					_hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2);
-				}
+				_hgt_sensor_offset = _terrain_vpos;
 			}
 
-		} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
+		} else if (!do_range_aid && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
 			setControlGPSHeight();
 			fuse_height = true;
 
@@ -1103,14 +1097,14 @@ void Ekf::controlHeightFusion()
 	}
 
 	if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _control_status.flags.rng_hgt
-	    && (!_range_data_ready || !_rng_hgt_valid)) {
+	    && (!_range_sensor.isDataHealthy())) {
 
 		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
 		// and are on the ground, then synthesise a measurement at the expected on ground value
 		if (!_control_status.flags.in_air) {
-			_range_sample_delayed.rng = _params.rng_gnd_clearance;
-			_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
-
+			_range_sensor.setRange(_params.rng_gnd_clearance);
+			_range_sensor.setDataReadiness(true);
+			_range_sensor.setValidity(true); // bypass the checks
 		}
 
 		fuse_height = true;
@@ -1163,15 +1157,14 @@ void Ekf::controlHeightFusion()
 			fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate,
 				gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio);
 
-		} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
-			// TODO: Tilt check does not belong here, should not set fuse height to true if tilted
+		} else if (_control_status.flags.rng_hgt) {
 			Vector2f rng_hgt_innov_gate;
 			Vector3f rng_hgt_obs_var;
 			// use range finder with tilt correction
-			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
+			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getRange() * _range_sensor.getCosTilt(),
 							 _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
-			rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
+			rng_hgt_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getRange())) * sq(_range_sensor.getCosTilt()), 0.01f);
 			// innovation gate size
 			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
 			// fuse height information
@@ -1197,7 +1190,7 @@ void Ekf::controlHeightFusion()
 void Ekf::checkRangeAidSuitability()
 {
 	if (_control_status.flags.in_air
-	    && _rng_hgt_valid
+	    && _range_sensor.isHealthy()
 	    && isTerrainEstimateValid()
 	    && isHorizontalAidingActive()) {
 		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 8443d242c3..b70dbda89d 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -75,8 +75,8 @@ void Ekf::reset()
 
 	_filter_initialised = false;
 	_terrain_initialised = false;
-	_sin_tilt_rng = sinf(_params.rng_sens_pitch);
-	_cos_tilt_rng = cosf(_params.rng_sens_pitch);
+	_range_sensor.setTiltOffset(_params.rng_sens_pitch);
+	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 
 	_control_status.value = 0;
 	_control_status_prev.value = 0;
diff --git a/EKF/ekf.h b/EKF/ekf.h
index e45bb8a846..e070204fcc 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -312,7 +312,6 @@ private:
 	} _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 
 	float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
-	float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
 
 	Vector3f _ang_rate_delayed_raw;	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
 
@@ -331,7 +330,6 @@ private:
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _mag_data_ready{false};	///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
-	bool _range_data_ready{false};	///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
 	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
@@ -348,7 +346,6 @@ private:
 	uint64_t _time_last_of_fuse{0};		///< time the last fusion of optical flow measurements were performed (uSec)
 	uint64_t _time_last_arsp_fuse{0};	///< time the last fusion of airspeed measurements were performed (uSec)
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
-	uint64_t _time_last_rng_ready{0};	///< time the last range finder measurement was ready (uSec)
 	uint64_t _time_last_mag{0};		///< measurement time of last magnetomter sample (uSec)
 	uint64_t _time_last_fake_pos{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 
@@ -491,17 +488,11 @@ private:
 	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint64_t _time_last_hagl_fuse{0};		///< last system time that the hagl measurement failed it's checks (uSec)
 	bool _terrain_initialised{false};	///< true when the terrain estimator has been initialized
-	float _sin_tilt_rng{0.0f};		///< sine of the range finder tilt rotation about the Y body axis
-	float _cos_tilt_rng{0.0f};		///< cosine of the range finder tilt rotation about the Y body axis
-	float _R_rng_to_earth_2_2{0.0f};	///< 2,2 element of the rotation matrix from sensor frame to earth frame
-	float _dt_last_range_update_filt_us{0.0f};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
 	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
 	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
-	bool _rng_hgt_valid{false};		///< true if range finder sample retrieved from buffer is valid
-	uint64_t _time_bad_rng_signal_quality{0};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
@@ -510,11 +501,6 @@ private:
 
 	// variables used to control range aid functionality
 	bool _is_range_aid_suitable{false};	///< true when range finder can be used in flight as the height reference instead of the primary height sensor
-	bool _range_aid_mode_selected{false};	///< true when range finder is being used as the height reference instead of the primary height sensor
-
-	// variables used to check range finder validity data
-	float _rng_stuck_min_val{0.0f};		///< minimum value for new rng measurement when being stuck
-	float _rng_stuck_max_val{0.0f};		///< maximum value for new rng measurement when being stuck
 
 	float _height_rate_lpf{0.0f};
 
@@ -745,12 +731,6 @@ private:
 	void checkRangeAidSuitability();
 	bool isRangeAidSuitable() { return _is_range_aid_suitable; }
 
-	// update _rng_hgt_valid, which indicates if the current range sample has passed validity checks
-	void updateRangeDataValidity();
-
-	// check for "stuck" range finder measurements when rng was not valid for certain period
-	void updateRangeDataStuck();
-
 	// return the square of two floating point numbers - used in auto coded sections
 	static constexpr float sq(float var) { return var * var; }
 
@@ -798,8 +778,6 @@ private:
 	// check that the range finder data is continuous
 	void updateRangeDataContinuity();
 
-	bool isRangeDataContinuous() { return _dt_last_range_update_filt_us < 2e6f; }
-
 	// Increase the yaw error variance of the quaternions
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index b8d367735d..69ac50769f 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -50,7 +50,7 @@
 bool Ekf::resetVelocity()
 {
 	// used to calculate the velocity change due to the reset
-	Vector3f vel_before_reset = _state.vel;
+	const Vector3f vel_before_reset = _state.vel;
 
 	// reset EKF states
 	if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
@@ -64,10 +64,10 @@ bool Ekf::resetVelocity()
 	} else if (_control_status.flags.opt_flow) {
 		ECL_INFO_TIMESTAMPED("reset velocity to flow");
 		// constrain height above ground to be above minimum possible
-		float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+		const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
 
 		// calculate absolute distance from focal point to centre of frame assuming a flat earth
-		float range = heightAboveGndEst / _R_rng_to_earth_2_2;
+		const float range = heightAboveGndEst / _range_sensor.getCosTilt();
 
 		if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
 			// we should have reliable OF measurements so
@@ -227,7 +227,7 @@ void Ekf::resetHeight()
 
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
-		float new_pos_down = _hgt_sensor_offset - _range_sample_delayed.rng * _R_rng_to_earth_2_2;
+		const float new_pos_down = _hgt_sensor_offset - _range_sensor.getRange() * _range_sensor.getCosTilt();
 
 		// update the state and associated variance
 		_state.pos(2) = new_pos_down;
@@ -1052,9 +1052,9 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed
 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
 {
 	// Calculate range finder limits
-	const float rangefinder_hagl_min = _rng_valid_min_val;
+	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
 	// Allow use of 75% of rangefinder maximum range to allow for angular motion
-	const float rangefinder_hagl_max = 0.75f * _rng_valid_max_val;
+	const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal();
 
 	// Calculate optical flow limits
 	// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index c9f524fcf6..09c666e1ff 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -47,6 +47,7 @@
 #include "AlphaFilter.hpp"
 #include "imu_down_sampler.hpp"
 #include "EKFGSF_yaw.h"
+#include "sensor_range_finder.hpp"
 
 #include <geo/geo.h>
 #include <matrix/math.hpp>
@@ -230,8 +231,7 @@ public:
 	// set sensor limitations reported by the rangefinder
 	void set_rangefinder_limits(float min_distance, float max_distance)
 	{
-		_rng_valid_min_val = min_distance;
-		_rng_valid_max_val = max_distance;
+		_range_sensor.setLimits(min_distance, max_distance);
 	}
 
 	// set sensor limitations reported by the optical flow sensor
@@ -436,7 +436,7 @@ protected:
 	magSample _mag_sample_delayed{};
 	baroSample _baro_sample_delayed{};
 	gpsSample _gps_sample_delayed{};
-	rangeSample _range_sample_delayed{};
+	sensor::SensorRangeFinder _range_sensor{};
 	airspeedSample _airspeed_sample_delayed{};
 	flowSample _flow_sample_delayed{};
 	extVisionSample _ev_sample_delayed{};
@@ -450,8 +450,6 @@ protected:
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
 
 	// Sensor limitations
-	float _rng_valid_min_val{0.0f};	///< minimum distance that the rangefinder can measure (m)
-	float _rng_valid_max_val{0.0f};	///< maximum distance that the rangefinder can measure (m)
 	float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
 	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
 	float _flow_max_distance{0.0f};	///< maximum distance that the optical flow sensor can operate at (m)
diff --git a/EKF/range_finder_checks.cpp b/EKF/range_finder_checks.cpp
deleted file mode 100644
index bf54e50a7a..0000000000
--- a/EKF/range_finder_checks.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 Estimation and Control Library (ECL). 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 ECL 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_finder_checks.cpp
- * Perform checks on range finder data in order to evaluate validity.
- *
- *
- */
-
-#include "ekf.h"
-
-// check that the range finder data is continuous
-void Ekf::updateRangeDataContinuity()
-{
-	// update range data continuous flag (1Hz ie 2000 ms)
-	/* Timing in micro seconds */
-
-	/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
-	float alpha = 0.5f * _dt_update;
-	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
-					(_imu_sample_delayed.time_us - _range_sample_delayed.time_us);
-
-	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 4e6f);
-}
-
-void Ekf::updateRangeDataValidity()
-{
-	updateRangeDataContinuity();
-
-	// check if out of date
-	if ((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) > 2 * RNG_MAX_INTERVAL) {
-		_rng_hgt_valid = false;
-		return;
-	}
-
-	// Don't allow faulty flag to clear unless range data is continuous
-	if (!_rng_hgt_valid && !isRangeDataContinuous()) {
-		return;
-	}
-
-	// Don't run the checks after this unless we have retrieved new data from the buffer
-	if (!_range_data_ready) {
-		return;
-	}
-
-	if (_range_sample_delayed.quality == 0) {
-		_time_bad_rng_signal_quality = _imu_sample_delayed.time_us;
-		_rng_hgt_valid = false;
-	} else if (_imu_sample_delayed.time_us - _time_bad_rng_signal_quality > (unsigned)_params.range_signal_hysteresis_ms) {
-		_rng_hgt_valid = true;
-	}
-
-	// Check if excessively tilted
-	if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) {
-		_rng_hgt_valid = false;
-		return;
-	}
-
-	// Check if out of range
-	if ((_range_sample_delayed.rng > _rng_valid_max_val)
-	|| (_range_sample_delayed.rng < _rng_valid_min_val)) {
-		if (_control_status.flags.in_air) {
-			_rng_hgt_valid = false;
-			return;
-		} else {
-			// Range finders can fail to provide valid readings when resting on the ground
-			// or being handled by the user, which prevents use of as a primary height sensor.
-			// To work around this issue, we replace out of range data with the expected on ground value.
-			_range_sample_delayed.rng = _params.rng_gnd_clearance;
-			return;
-		}
-	}
-
-	updateRangeDataStuck();
-
-	_rng_hgt_valid = _rng_hgt_valid && !_control_status.flags.rng_stuck;
-}
-
-void Ekf::updateRangeDataStuck()
-{
-	// 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 (((_range_sample_delayed.time_us - _time_last_rng_ready) > (uint64_t)10e6) &&
-	    _control_status.flags.in_air) {
-
-		// require a variance of rangefinder values to check for "stuck" measurements
-		if (_rng_stuck_max_val - _rng_stuck_min_val > _params.range_stuck_threshold) {
-			_time_last_rng_ready = _range_sample_delayed.time_us;
-			_rng_stuck_min_val = 0.0f;
-			_rng_stuck_max_val = 0.0f;
-			_control_status.flags.rng_stuck = false;
-
-		} else {
-			if (_range_sample_delayed.rng > _rng_stuck_max_val) {
-				_rng_stuck_max_val = _range_sample_delayed.rng;
-			}
-
-			if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) {
-				_rng_stuck_min_val = _range_sample_delayed.rng;
-			}
-
-			_control_status.flags.rng_stuck = true;
-		}
-
-	} else {
-		_time_last_rng_ready = _range_sample_delayed.time_us;
-	}
-}
diff --git a/EKF/sensor_range_finder.cpp b/EKF/sensor_range_finder.cpp
new file mode 100644
index 0000000000..afa8a2b263
--- /dev/null
+++ b/EKF/sensor_range_finder.cpp
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 Estimation and Control Library (ECL). 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 ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sensor_range_finder.cpp
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ *
+ */
+
+#include "sensor_range_finder.hpp"
+
+namespace estimator
+{
+namespace sensor
+{
+
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth)
+{
+	updateSensorToEarthRotation(R_to_earth);
+	updateValidity(current_time_us);
+}
+
+void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth)
+{
+	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
+	// this is required for use of range finder and flow data
+	_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_tilt_rng + R_to_earth(2, 2) * _cos_tilt_rng;
+}
+
+void SensorRangeFinder::updateValidity(uint64_t current_time_us)
+{
+	updateDtDataLpf(current_time_us);
+
+	if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
+		_is_sample_valid = false;
+		return;
+	}
+
+	// Don't run the checks unless we have retrieved new data from the buffer
+	if (_is_sample_ready) {
+		_is_sample_valid = false;
+
+		if (_sample.quality == 0) {
+			_time_bad_quality_us = current_time_us;
+
+		} else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) {
+			// We did not receive bad quality data for some time
+
+			if (isTiltOk() && isDataInRange()) {
+				updateStuckCheck();
+
+				if (!_is_stuck) {
+					_is_sample_valid = true;
+					_time_last_valid_us = _sample.time_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()
+{
+	// 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;
+		}
+	}
+}
+
+} // namespace sensor
+} // namespace estimator
diff --git a/EKF/sensor_range_finder.hpp b/EKF/sensor_range_finder.hpp
new file mode 100644
index 0000000000..7eb7592648
--- /dev/null
+++ b/EKF/sensor_range_finder.hpp
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 Estimation and Control Library (ECL). 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 ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sensor_range_finder.hpp
+ * Range finder class containing all the required checks
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ *
+ */
+#pragma once
+
+#include "Sensor.hpp"
+#include <matrix/math.hpp>
+
+namespace estimator
+{
+namespace sensor
+{
+
+class SensorRangeFinder : public Sensor
+{
+public:
+	SensorRangeFinder() = default;
+	~SensorRangeFinder() override = default;
+
+	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
+	bool isHealthy() const override { return _is_sample_valid; }
+	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
+
+	void setSample(const rangeSample &sample) {
+		_sample = sample;
+		_is_sample_ready = true;
+	}
+
+	// This is required because of the ring buffer
+	// TODO: move the ring buffer here
+	rangeSample* getSampleAddress() { return &_sample; }
+
+	void setTiltOffset(float new_tilt_offset)
+	{
+		if (fabsf(_tilt_offset_rad - new_tilt_offset) > FLT_EPSILON) {
+			_sin_tilt_rng = sinf(new_tilt_offset);
+			_cos_tilt_rng = cosf(new_tilt_offset);
+			_tilt_offset_rad = new_tilt_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;
+	}
+
+	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
+
+	void setRange(float rng) { _sample.rng = rng; }
+	float getRange() const { return _sample.rng; }
+
+	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; }
+
+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;
+	void updateStuckCheck();
+
+	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
+	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
+
+	/*
+	 * 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)
+	float _stuck_min_val{};		///< minimum value for new rng measurement when being stuck
+	float _stuck_max_val{};		///< maximum value for new rng measurement when being stuck
+
+	/*
+	 * Data regularity check
+	 */
+	static constexpr float _dt_update{0.01f}; 	///< delta time since last ekf update TODO: this should be a parameter
+	float _dt_data_lpf{};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
+
+	/*
+	 * Tilt check
+	 */
+	float _cos_tilt_rng_to_earth{};		///< 2,2 element of the rotation matrix from sensor frame to earth frame
+	float _range_cos_max_tilt{0.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
+	float _tilt_offset_rad{3.14f}; 		///< range finder tilt rotation about the Y body axis
+	float _sin_tilt_rng{0.0f}; 		///< sine of the range finder tilt rotation about the Y body axis
+	float _cos_tilt_rng{-1.0f}; 		///< cosine of the range finder tilt rotation about the Y body axis
+
+	/*
+	 * Range check
+	 */
+	float _rng_valid_min_val{};	///< minimum distance that the rangefinder can measure (m)
+	float _rng_valid_max_val{};	///< maximum distance that the rangefinder can measure (m)
+
+	/*
+	 * Quality check
+	 */
+	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
+	uint64_t _quality_hyst_us{1000000}; 	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+};
+
+} // namespace sensor
+} // namespace estimator
diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index 9465c86489..74986f4d51 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -57,11 +57,11 @@ bool Ekf::initHagl()
 		initialized = true;
 
 	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
-		   && _rng_hgt_valid
-		   && isRecent(latest_measurement.time_us, (uint64_t)2e5)
-		   && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
+		   && _range_sensor.isHealthy()
+		   && isRecent(latest_measurement.time_us, (uint64_t)2e5)) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
-		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
+		// TODO: the latest measurment did not go through the checks and could be invalid!
+		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _range_sensor.getCosTilt();
 		// initialise state variance to variance of measurement
 		_terrain_var = sq(_params.range_noise);
 		// success
@@ -113,13 +113,8 @@ void Ekf::runTerrainEstimator()
 		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
 
 		// Fuse range finder data if available
-		if (_range_data_ready && _rng_hgt_valid) {
+		if (_range_sensor.isDataHealthy()) {
 			fuseHagl();
-
-			// update range sensor angle parameters in case they have changed
-			// we do this here to avoid doing those calculations at a high rate
-			_sin_tilt_rng = sinf(_params.rng_sens_pitch);
-			_cos_tilt_rng = cosf(_params.rng_sens_pitch);
 		}
 
 		if (_flow_for_terrain_data_ready) {
@@ -138,53 +133,47 @@ void Ekf::runTerrainEstimator()
 
 void Ekf::fuseHagl()
 {
-	// If the vehicle is excessively tilted, do not try to fuse range finder observations
-	if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
-		// get a height above ground measurement from the range finder assuming a flat earth
-		const float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;
+	// get a height above ground measurement from the range finder assuming a flat earth
+	const float meas_hagl = _range_sensor.getRange() * _range_sensor.getCosTilt();
 
-		// predict the hagl from the vehicle position and terrain height
-		const float pred_hagl = _terrain_vpos - _state.pos(2);
+	// predict the hagl from the vehicle position and terrain height
+	const float pred_hagl = _terrain_vpos - _state.pos(2);
 
-		// calculate the innovation
-		_hagl_innov = pred_hagl - meas_hagl;
+	// calculate the innovation
+	_hagl_innov = pred_hagl - meas_hagl;
 
-		// calculate the observation variance adding the variance of the vehicles own height uncertainty
-		const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
-				     + sq(_params.range_noise)
-				     + sq(_params.range_noise_scaler * _range_sample_delayed.rng);
+	// calculate the observation variance adding the variance of the vehicles own height uncertainty
+	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
+			     + sq(_params.range_noise)
+			     + sq(_params.range_noise_scaler * _range_sensor.getRange());
 
-		// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
-		_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
+	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
+	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
 
-		// perform an innovation consistency check and only fuse data if it passes
-		const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
-		_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
+	// perform an innovation consistency check and only fuse data if it passes
+	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
+	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
 
-		if (_hagl_test_ratio <= 1.0f) {
-			// calculate the Kalman gain
-			float gain = _terrain_var / _hagl_innov_var;
-			// correct the state
-			_terrain_vpos -= gain * _hagl_innov;
-			// correct the variance
-			_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-			// record last successful fusion event
-			_time_last_hagl_fuse = _time_last_imu;
-			_innov_check_fail_status.flags.reject_hagl = false;
-
-		} else {
-			// If we have been rejecting range data for too long, reset to measurement
-			if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
-				_terrain_vpos = _state.pos(2) + meas_hagl;
-				_terrain_var = obs_variance;
-
-			} else {
-				_innov_check_fail_status.flags.reject_hagl = true;
-			}
-		}
+	if (_hagl_test_ratio <= 1.0f) {
+		// calculate the Kalman gain
+		float gain = _terrain_var / _hagl_innov_var;
+		// correct the state
+		_terrain_vpos -= gain * _hagl_innov;
+		// correct the variance
+		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
+		// record last successful fusion event
+		_time_last_hagl_fuse = _time_last_imu;
+		_innov_check_fail_status.flags.reject_hagl = false;
 
 	} else {
-		_innov_check_fail_status.flags.reject_hagl = true;
+		// If we have been rejecting range data for too long, reset to measurement
+		if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
+			_terrain_vpos = _state.pos(2) + meas_hagl;
+			_terrain_var = obs_variance;
+
+		} else {
+			_innov_check_fail_status.flags.reject_hagl = true;
+		}
 	}
 }
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 048b68a811..9d5b4bc2ca 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -48,6 +48,7 @@ set(SRCS
 	test_EKF_externalVision.cpp
 	test_EKF_airspeed.cpp
 	test_EKF_withReplayData.cpp
+	test_SensorRangeFinder.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})
 
diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp
index 07eb8ca128..79f596546b 100644
--- a/test/sensor_simulator/range_finder.cpp
+++ b/test/sensor_simulator/range_finder.cpp
@@ -17,6 +17,7 @@ void RangeFinder::send(uint64_t time)
 {
 	_range_sample.time_us = time;
 	_ekf->setRangeData(_range_sample);
+	_ekf->set_rangefinder_limits(0.2f, 20.f); // This usually comes from the driver
 }
 
 void RangeFinder::setData(float range_data_meters, int8_t range_quality)
diff --git a/test/test_SensorRangeFinder.cpp b/test/test_SensorRangeFinder.cpp
new file mode 100644
index 0000000000..662cd28e10
--- /dev/null
+++ b/test/test_SensorRangeFinder.cpp
@@ -0,0 +1,299 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 ECL 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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <math.h>
+#include "EKF/common.h"
+#include "EKF/sensor_range_finder.hpp"
+#include <matrix/math.hpp>
+
+using estimator::rangeSample;
+using matrix::Dcmf;
+using matrix::Eulerf;
+using namespace estimator::sensor;
+
+class SensorRangeFinderTest : public ::testing::Test {
+public:
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		_range_finder.setTiltOffset(0.f);
+		_range_finder.setCosMaxTilt(0.707f);
+		_range_finder.setLimits(_min_range, _max_range);
+	}
+
+	// Use this method to clean up any memory, network etc. after each test
+	void TearDown() override
+	{
+	}
+
+protected:
+	SensorRangeFinder _range_finder{};
+	const rangeSample _good_sample{1.f, (uint64_t)2e6, 100}; // {range, time_us, quality}
+	const float _min_range{0.5f};
+	const float _max_range{10.f};
+
+	void updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us);
+	void testTilt(const Eulerf &euler, bool should_pass);
+};
+
+void SensorRangeFinderTest::updateSensorAtRate(uint64_t duration_us, uint64_t dt_update_us, uint64_t dt_sensor_us)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	rangeSample new_sample = _good_sample;
+	uint64_t t_now_us = _good_sample.time_us;
+	for (int i = 0; i < int(duration_us / dt_update_us); i++) {
+		t_now_us += dt_update_us;
+		if ((i % int(dt_sensor_us / dt_update_us)) == 0) {
+			new_sample.rng += 0.2f; // update the range to not trigger the stuck detection
+			if (new_sample.rng > _max_range) {
+				new_sample.rng = _min_range;
+			}
+			new_sample.time_us = t_now_us;
+			_range_finder.setSample(new_sample);
+		}
+		_range_finder.runChecks(t_now_us, attitude);
+	}
+}
+
+void SensorRangeFinderTest::testTilt(const Eulerf &euler, bool should_pass)
+{
+	const Dcmf attitude{euler};
+	_range_finder.setSample(_good_sample);
+	_range_finder.runChecks(_good_sample.time_us, attitude);
+
+	if (should_pass) {
+		EXPECT_TRUE(_range_finder.isDataHealthy());
+		EXPECT_TRUE(_range_finder.isHealthy());
+
+	} else {
+		EXPECT_FALSE(_range_finder.isDataHealthy());
+		EXPECT_FALSE(_range_finder.isHealthy());
+	}
+}
+
+TEST_F(SensorRangeFinderTest, setRange)
+{
+	rangeSample sample{};
+	sample.rng = 1.f;
+	sample.time_us = 1e6;
+	sample.quality = 9;
+
+	_range_finder.setRange(sample.rng);
+	_range_finder.setDataReadiness(true);
+	_range_finder.setValidity(true);
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, goodData)
+{
+	// WHEN: the drone is leveled and the data is good
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+	_range_finder.setSample(_good_sample);
+	_range_finder.runChecks(_good_sample.time_us, attitude);
+
+	// THEN: the data can be used for aiding
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, tiltExceeded)
+{
+	const Eulerf zero(0.f, 0.f, 0.f);
+	const Eulerf pitch_46(0.f, 0.8f, 0.f);
+	const Eulerf pitch_minus46(0.f, -0.8f, 0.f);
+	const Eulerf pitch_40(0.f, 0.7f, 1.f);
+	const Eulerf pitch_minus40(0.f, -0.7f, 0.f);
+	const Eulerf roll_46(0.8f, 0.f, 0.f);
+	const Eulerf roll_minus46(-0.8f, 0.f, 0.f);
+	const Eulerf roll_40(0.7f, 0.f, 2.f);
+	const Eulerf roll_minus40(-0.7f, 0.f, 3.f);
+	const Eulerf roll_28_pitch_minus28(0.5f, -0.5f, 4.f);
+	const Eulerf roll_46_pitch_minus46(0.8f, -0.8f, 4.f);
+
+	testTilt(zero, true);
+	testTilt(pitch_46, false);
+	testTilt(pitch_minus46, false);
+	testTilt(pitch_40, true);
+	testTilt(pitch_minus40, true);
+	testTilt(roll_46, false);
+	testTilt(roll_minus46, false);
+	testTilt(roll_40, true);
+	testTilt(roll_minus40, true);
+	testTilt(roll_28_pitch_minus28, true);
+	testTilt(roll_46_pitch_minus46, false);
+
+}
+
+TEST_F(SensorRangeFinderTest, rangeMaxExceeded)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the measured range is larger than the maximum
+	rangeSample bad_sample = _good_sample;
+	bad_sample.rng = _max_range + 0.01f;
+	_range_finder.setSample(bad_sample);
+	_range_finder.runChecks(bad_sample.time_us, attitude);
+
+	// THEN: the data should be marked as unhealthy
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, rangeMinExceeded)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the measured range is shorter than the minimum
+	rangeSample bad_sample = _good_sample;
+	bad_sample.rng = _min_range - 0.01f;
+	_range_finder.setSample(bad_sample);
+	_range_finder.runChecks(bad_sample.time_us, attitude);
+
+	// THEN: the data should be marked as unhealthy
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, outOfDate)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the data is outdated
+	rangeSample outdated_sample = _good_sample;
+	outdated_sample.time_us = 0;
+	uint64_t t_now = _good_sample.time_us;
+	_range_finder.setSample(outdated_sample);
+	_range_finder.runChecks(t_now, attitude);
+
+	// THEN: the data should be marked as unhealthy
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, rangeStuck)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the data is first not valid and then
+	// constantly the same
+	rangeSample new_sample = _good_sample;
+	const uint64_t dt = 3e5;
+	const uint64_t stuck_timeout = 11e6;
+	new_sample.quality = 0;
+	for (int i = 0; i < int(stuck_timeout / dt); i++) {
+		_range_finder.setSample(new_sample);
+		_range_finder.runChecks(new_sample.time_us, attitude);
+		new_sample.time_us += dt;
+	}
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	new_sample.quality = 100;
+	// we need a few sample to pass the hysteresis check
+	for (int i = 0; i < int(2e6 / dt); i++) {
+		_range_finder.setSample(new_sample);
+		_range_finder.runChecks(new_sample.time_us, attitude);
+		new_sample.time_us += dt;
+	}
+
+	// THEN: the data should be marked as unhealthy
+	// because the sensor is "stuck"
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, qualityHysteresis)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the data is first bad and then good
+	rangeSample new_sample = _good_sample;
+
+	new_sample.quality = 0;
+	_range_finder.setSample(new_sample);
+	_range_finder.runChecks(new_sample.time_us, attitude);
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	new_sample.quality = _good_sample.quality;
+	_range_finder.setSample(new_sample);
+	_range_finder.runChecks(new_sample.time_us, attitude);
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	// AND: we need to put enough good data to pass the hysteresis
+	const uint64_t dt = 3e5;
+	const uint64_t hyst_time = 1e6;
+	for (int i = 0; i < int(hyst_time / dt) + 2; i++) {
+		_range_finder.setSample(new_sample);
+		_range_finder.runChecks(new_sample.time_us, attitude);
+		new_sample.time_us += dt;
+	}
+
+	// THEN: the data is again declared healthy
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+}
+
+TEST_F(SensorRangeFinderTest, continuity)
+{
+	const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)};
+
+	// WHEN: the data rate is too slow
+	const uint64_t dt_update_us = 10e3;
+	uint64_t dt_sensor_us = 4e6;
+	uint64_t duration_us = 8e6;
+	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+
+	// THEN: the data should be marked as unhealthy
+	// Note that it also fails the out-of-date test here
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	// AND WHEN: the data rate is acceptable
+	dt_sensor_us = 3e5;
+	duration_us = 5e5;
+	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+
+	// THEN: it should still fail until the filter converge
+	// to the new datarate
+	EXPECT_FALSE(_range_finder.isDataHealthy());
+	EXPECT_FALSE(_range_finder.isHealthy());
+
+	updateSensorAtRate(duration_us, dt_update_us, dt_sensor_us);
+	EXPECT_TRUE(_range_finder.isDataHealthy());
+	EXPECT_TRUE(_range_finder.isHealthy());
+}