diff --git a/src/lib/mathlib/math/filter/MedianFilter.hpp b/src/lib/mathlib/math/filter/MedianFilter.hpp
index b483e59e7c..227a8d729c 100644
--- a/src/lib/mathlib/math/filter/MedianFilter.hpp
+++ b/src/lib/mathlib/math/filter/MedianFilter.hpp
@@ -78,6 +78,8 @@ public:
 		return median();
 	}
 
+	size_t window_size() const { return WINDOW; }
+
 private:
 
 	static int cmp(const void *a, const void *b)
diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/modules/ekf2/EKF/RingBuffer.h
index 512b3a6b6a..1a686d61e8 100644
--- a/src/modules/ekf2/EKF/RingBuffer.h
+++ b/src/modules/ekf2/EKF/RingBuffer.h
@@ -153,6 +153,27 @@ public:
 		return false;
 	}
 
+	bool peek_first_older_than(const uint64_t &timestamp, data_type *sample) const
+	{
+		// start looking from newest observation data
+		for (uint8_t i = 0; i < _size; i++) {
+			int index = (_head - i);
+			index = index < 0 ? _size + index : index;
+
+			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
+				*sample = _buffer[index];
+				return true;
+			}
+
+			if (index == _tail) {
+				// we have reached the tail and haven't got a match
+				return false;
+			}
+		}
+
+		return false;
+	}
+
 	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
 
 private:
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index eaa4b2da0a..40bdb5a0da 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -94,7 +94,16 @@ void Ekf::controlFusionModes()
 
 	// check for arrival of new sensor data at the fusion time horizon
 	_time_prev_gps_us = _gps_sample_delayed.time_us;
+
+	const uint64_t delta_time_prev_gps_us = _gps_sample_delayed.time_us;
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
+
+	// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
+	if (_gps_data_ready && (delta_time_prev_gps_us != 0)) {
+		_delta_time_gps_us = _gps_sample_delayed.time_us - delta_time_prev_gps_us;
+	}
+
+
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
 
 	if (_mag_data_ready) {
@@ -114,20 +123,28 @@ void Ekf::controlFusionModes()
 		}
 	}
 
-	_delta_time_baro_us = _baro_sample_delayed.time_us;
+
+	const uint64_t delta_time_prev_baro_us = _baro_sample_delayed.time_us;
 	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
 
 	// if we have a new baro sample save the delta time between this sample and the last sample which is
 	// used below for baro offset calculations
-	if (_baro_data_ready) {
-		_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
+	if (_baro_data_ready && (delta_time_prev_baro_us != 0)) {
+		_delta_time_baro_us = _baro_sample_delayed.time_us - delta_time_prev_baro_us;
 	}
 
+
 	{
 		// Get range data from buffer and check validity
+		const uint64_t delta_time_prev_rng_us = _range_sensor.sample().time_us;
 		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);
 
+		// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
+		if (is_rng_data_ready && (delta_time_prev_rng_us != 0)) {
+			_delta_time_rng_us = _range_sensor.sample().time_us - delta_time_prev_rng_us;
+		}
+
 		// 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);
@@ -160,7 +177,16 @@ void Ekf::controlFusionModes()
 		_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
 	}
 
+
+	const uint64_t delta_time_prev_ev_us = _ev_sample_delayed.time_us;
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
+
+	// if we have a new EV sample save the delta time between this sample and the last sample which is used for height offset calculations
+	if (_ev_data_ready && (delta_time_prev_ev_us != 0)) {
+		_delta_time_ev_us = _ev_sample_delayed.time_us - delta_time_prev_ev_us;
+	}
+
+
 	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
 
 	// check for height sensor timeouts and reset and change sensor if necessary
@@ -664,9 +690,7 @@ void Ekf::controlHeightSensorTimeouts()
 			const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
 
 			// check the baro height source for consistency and freshness
-			const baroSample &baro_init = _baro_buffer.get_newest();
-			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
-			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);
+			const bool baro_data_consistent = (_baro_hgt_test_ratio(1) < 1.f); // TODO: review
 
 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
 			const bool reset_to_baro = !_baro_hgt_faulty &&
@@ -819,18 +843,11 @@ void Ekf::controlHeightFusion()
 			// 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) {
-				_hgt_sensor_offset = _terrain_vpos;
+				_rng_hgt_offset = _terrain_vpos;
 			}
 
 		} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
 			startBaroHgtFusion();
-
-		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
-			// we have just switched to using gps height, calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
-				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
-			}
 		}
 
 		break;
@@ -844,13 +861,13 @@ void Ekf::controlHeightFusion()
 				// measurement matches our current height estimate
 				// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
 				if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-					_hgt_sensor_offset = _terrain_vpos;
+					_rng_hgt_offset = _terrain_vpos;
 
 				} else if (_control_status.flags.in_air) {
-					_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+					_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
 
 				} else {
-					_hgt_sensor_offset = _params.rng_gnd_clearance;
+					_rng_hgt_offset = _params.rng_gnd_clearance;
 				}
 			}
 		}
@@ -869,7 +886,7 @@ void Ekf::controlHeightFusion()
 
 				// we have just switched to using range finder, calculate height sensor offset such that current
 				// measurement matches our current height estimate
-				_hgt_sensor_offset = _terrain_vpos;
+				_rng_hgt_offset = _terrain_vpos;
 			}
 
 		} else {
@@ -907,43 +924,38 @@ void Ekf::controlHeightFusion()
 	}
 
 	updateBaroHgtBias();
-	updateBaroHgtOffset();
 
-	if (_control_status.flags.baro_hgt) {
+	// baro
+	if (_baro_data_ready && !_baro_hgt_faulty) {
+		fuseBaroHgt();
+	}
 
-		if (_baro_data_ready && !_baro_hgt_faulty) {
-			fuseBaroHgt();
-		}
+	// GPS
+	if (_gps_data_ready) {
+		fuseGpsHgt();
+	}
 
-	} else if (_control_status.flags.gps_hgt) {
+	// range
+	if (_range_sensor.isDataHealthy()) {
+		fuseRngHgt();
 
-		if (_gps_data_ready) {
-			fuseGpsHgt();
-		}
+	} else if (_control_status.flags.rng_hgt
+			&&!_control_status.flags.in_air
+			&& _range_sensor.isRegularlySendingData()
+			&& isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
 
-	} else if (_control_status.flags.rng_hgt) {
+		// 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
+		_range_sensor.setRange(_params.rng_gnd_clearance);
+		_range_sensor.setDataReadiness(true);
+		_range_sensor.setValidity(true); // bypass the checks
 
-		if (_range_sensor.isDataHealthy()) {
-			fuseRngHgt();
+		fuseRngHgt();
+	}
 
-		} else if (!_control_status.flags.in_air
-			   && _range_sensor.isRegularlySendingData()
-			   && isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)) {
-
-			// 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
-			_range_sensor.setRange(_params.rng_gnd_clearance);
-			_range_sensor.setDataReadiness(true);
-			_range_sensor.setValidity(true); // bypass the checks
-
-			fuseRngHgt();
-		}
-
-	} else if (_control_status.flags.ev_hgt) {
-
-		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-			fuseEvHgt();
-		}
+	// vision
+	if (_control_status.flags.ev_hgt && _ev_data_ready) {
+		fuseEvHgt();
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index c506b4f7cd..f5a3dff7b2 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -89,16 +89,17 @@ bool Ekf::update()
 {
 	bool updated = false;
 
-	if (!_filter_initialised) {
-		_filter_initialised = initialiseFilter();
-
-		if (!_filter_initialised) {
-			return false;
-		}
-	}
-
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
+
+		if (!_filter_initialised) {
+			_filter_initialised = initialiseFilter();
+
+			if (!_filter_initialised) {
+				return false;
+			}
+		}
+
 		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
@@ -115,59 +116,30 @@ bool Ekf::update()
 		runYawEKFGSF();
 	}
 
-	// the output observer always runs
-	// Use full rate IMU data at the current time horizon
-	calculateOutputStates(_newest_high_rate_imu_sample);
+	if (_filter_initialised) {
+		// the output observer always runs
+		// Use full rate IMU data at the current time horizon
+		calculateOutputStates(_newest_high_rate_imu_sample);
+	}
 
 	return updated;
 }
 
 bool Ekf::initialiseFilter()
 {
-	// Filter accel for tilt initialization
-	const imuSample &imu_init = _imu_buffer.get_newest();
-
 	// protect against zero data
-	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
+	if (_imu_sample_delayed.delta_vel_dt < 1e-4f || _imu_sample_delayed.delta_ang_dt < 1e-4f) {
 		return false;
 	}
 
 	if (_is_first_imu_sample) {
-		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
+		_accel_lpf.reset(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
+		_gyro_lpf.reset(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
 		_is_first_imu_sample = false;
 
 	} else {
-		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
-	}
-
-	// Sum the magnetometer measurements
-	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		if (_mag_sample_delayed.time_us != 0) {
-			if (_mag_counter == 0) {
-				_mag_lpf.reset(_mag_sample_delayed.mag);
-
-			} else {
-				_mag_lpf.update(_mag_sample_delayed.mag);
-			}
-
-			_mag_counter++;
-		}
-	}
-
-	// accumulate enough height measurements to be confident in the quality of the data
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		if (_baro_sample_delayed.time_us != 0) {
-			if (_baro_counter == 0) {
-				_baro_hgt_offset = _baro_sample_delayed.hgt;
-
-			} else {
-				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
-			}
-
-			_baro_counter++;
-		}
+		_accel_lpf.update(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
+		_gyro_lpf.update(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
 	}
 
 	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
@@ -177,13 +149,62 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_baro_counter < _obs_buffer_length) {
-		// not enough baro samples accumulated
-		return false;
+	if (_baro_hgt_counter > 0) {
+		_baro_hgt_offset = _baro_hgt_lpf.getState();
 	}
 
-	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
-	setControlBaroHeight();
+	if (_gps_hgt_counter > 0) {
+		_gps_hgt_offset = _gps_hgt_lpf.getState();
+	}
+
+	if (_rng_hgt_counter >= _rng_hgt_filter.window_size()) {
+		_rng_hgt_offset = _rng_hgt_filter.median();
+	}
+
+	if (_ev_hgt_counter > 0) {
+		_ev_hgt_offset = _ev_hgt_lpf.getState();
+	}
+
+	switch (_params.vdist_sensor_type) {
+	default:
+
+	// FALLTHROUGH
+	case VDIST_SENSOR_BARO:
+		if (_baro_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlBaroHeight();
+		break;
+
+	case VDIST_SENSOR_GPS:
+		if (_gps_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlGPSHeight();
+		break;
+
+	case VDIST_SENSOR_RANGE:
+		if (_rng_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlRangeHeight();
+		break;
+
+	case VDIST_SENSOR_EV:
+		if (_ev_hgt_counter < _obs_buffer_length) {
+			// not enough samples accumulated
+			return false;
+		}
+
+		setControlEVHeight();
+		break;
+	}
 
 	if (!initialiseTilt()) {
 		return false;
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index c757fe092b..3fef458bc0 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -391,7 +391,11 @@ private:
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
+
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
+	uint64_t _delta_time_gps_us{0};
+	uint64_t _delta_time_rng_us{0};
+	uint64_t _delta_time_ev_us{0};
 
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
 
@@ -504,15 +508,17 @@ private:
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
-	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
-	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
-	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+
+	float _baro_hgt_offset{NAN};		///< baro height reading at the local NED origin (m)
+	float _gps_hgt_offset{NAN};
+	float _rng_hgt_offset{NAN};
+	float _ev_hgt_offset{NAN};
+
 	float _baro_hgt_bias{0.0f};
 	float _baro_hgt_bias_var{1.f};
 
@@ -674,7 +680,7 @@ private:
 				    Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
 
 	bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
-				  Vector3f &innov_var, Vector2f &test_ratio);
+				  Vector3f &innov_var, Vector2f &test_ratio, bool fuse = true);
 
 	void fuseGpsVelPos();
 
@@ -916,7 +922,6 @@ private:
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();
 
-	void updateBaroHgtOffset();
 	void updateBaroHgtBias();
 
 	// return an estimation of the GPS altitude variance
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 4c5086c346..4de9c25eb6 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -255,9 +255,6 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
-	// Get the most recent GPS data
-	const gpsSample &gps_newest = _gps_buffer.get_newest();
-
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
 
@@ -265,33 +262,28 @@ void Ekf::resetHeight()
 		if (!_control_status_prev.flags.rng_hgt) {
 
 			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-				_hgt_sensor_offset = _terrain_vpos;
+				_rng_hgt_offset = _terrain_vpos;
 
 			} else if (_control_status.flags.in_air) {
-				_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+				_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
 
 			} else {
-				_hgt_sensor_offset = _params.rng_gnd_clearance;
+				_rng_hgt_offset = _params.rng_gnd_clearance;
 			}
 
 		}
 
 		// update the state and associated variance
-		resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());
+		resetVerticalPositionTo(_rng_hgt_offset - _range_sensor.getDistBottom());
 
 		// the state variance is the same as the observation
 		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
 
-		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-		const baroSample &baro_newest = _baro_buffer.get_newest();
-		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
-
 	} else if (_control_status.flags.baro_hgt) {
-		// initialize vertical position with newest baro measurement
-		const baroSample &baro_newest = _baro_buffer.get_newest();
 
 		if (!_baro_hgt_faulty) {
-			resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);
+			// initialize vertical position with newest baro measurement
+			resetVerticalPositionTo(-_baro_hgt_lpf.getState() + _baro_hgt_offset);
 
 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -303,39 +295,65 @@ void Ekf::resetHeight()
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
 		if (!_gps_hgt_intermittent) {
-			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);
+			gpsSample gps_sample;
+
+			if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
+				gps_sample = _gps_buffer.get_newest();
+			}
+
+			resetVerticalPositionTo(_gps_hgt_offset - _gps_hgt_lpf.getState() + _gps_alt_ref);
 
 			// the state variance is the same as the observation
-			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
-
-			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
-			const baroSample &baro_newest = _baro_buffer.get_newest();
-			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_sample.vacc));
 
 		} else {
 			// TODO: reset to last known gps based estimate
 		}
 
 	} else if (_control_status.flags.ev_hgt) {
-		// initialize vertical position with newest measurement
-		const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
+		extVisionSample ext_vision_sample;
 
-		// use the most recent data if it's time offset from the fusion time horizon is smaller
-		if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
-			resetVerticalPositionTo(ev_newest.pos(2));
-
-		} else {
-			resetVerticalPositionTo(_ev_sample_delayed.pos(2));
+		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
+			ext_vision_sample = _ext_vision_buffer.get_newest();
 		}
+
+		resetVerticalPositionTo(_ev_hgt_lpf.getState() + _ev_hgt_offset);
+
+		P.uncorrelateCovarianceSetVariance<1>(9, sq(ext_vision_sample.posVar(2)));
 	}
 
+	// reset the height sensor offsets which is subtracted from the readings if we need to use it as a backup
+	_baro_hgt_offset = _state.pos(2) + _baro_hgt_lpf.getState();
+	_gps_hgt_offset  = _state.pos(2) + _gps_hgt_lpf.getState() - _gps_alt_ref;
+	_rng_hgt_offset  = _state.pos(2) + _range_sensor.getDistBottom();
+	_ev_hgt_offset   = _state.pos(2) + _ev_hgt_lpf.getState();
+
+
 	// reset the vertical velocity state
 	if (_control_status.flags.gps && !_gps_hgt_intermittent) {
+		gpsSample gps_sample;
+
+		if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
+			gps_sample = _gps_buffer.get_newest();
+		}
+
 		// If we are using GPS, then use it to reset the vertical velocity
-		resetVerticalVelocityTo(gps_newest.vel(2));
+		resetVerticalVelocityTo(gps_sample.vel(2));
 
 		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));
+		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
+
+	} else if (_control_status.flags.ev_vel) {
+		extVisionSample ext_vision_sample;
+
+		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
+			ext_vision_sample = _ext_vision_buffer.get_newest();
+		}
+
+		// use the most recent data if it's time offset from the fusion time horizon is smaller
+		resetVerticalVelocityTo(ext_vision_sample.vel(2));
+
+		P.uncorrelateCovarianceSetVariance<1>(6, math::max(getVisionVelocityVarianceInEkfFrame()(2), sq(0.05f)));
 
 	} else {
 		// we don't know what the vertical velocity is, so set it to zero
@@ -1281,10 +1299,6 @@ void Ekf::startBaroHgtFusion()
 {
 	setControlBaroHeight();
 
-	// We don't need to set a height sensor offset
-	// since we track a separate _baro_hgt_offset
-	_hgt_sensor_offset = 0.0f;
-
 	// Turn off ground effect compensation if it times out
 	if (_control_status.flags.gnd_effect) {
 		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
@@ -1297,27 +1311,6 @@ void Ekf::startBaroHgtFusion()
 void Ekf::startGpsHgtFusion()
 {
 	setControlGPSHeight();
-
-	// we have just switched to using gps height, calculate height sensor offset such that current
-	// measurement matches our current height estimate
-	if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
-		_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
-	}
-}
-
-void Ekf::updateBaroHgtOffset()
-{
-	// calculate a filtered offset between the baro origin and local NED origin if we are not
-	// using the baro as a height reference
-	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
-		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
-
-		// apply a 10 second first order low pass filter to baro offset
-		const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
-
-		const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
-		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
-	}
 }
 
 float Ekf::getGpsHeightVariance()
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
index b68c034988..e00b64f4e0 100644
--- a/src/modules/ekf2/EKF/estimator_interface.cpp
+++ b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -144,6 +144,16 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		_mag_data_sum.setZero();
 		_mag_timestamp_sum = 0;
 	}
+
+
+	if (_mag_counter == 0) {
+		_mag_lpf.reset(mag_sample.mag);
+
+	} else {
+		_mag_lpf.update(mag_sample.mag);
+	}
+
+	_mag_counter++;
 }
 
 void EstimatorInterface::setGpsData(const gps_message &gps)
@@ -204,6 +214,20 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 
 		_gps_buffer.push(gps_sample_new);
 	}
+
+	if (gps.fix_type > 2) {
+		// accumulate enough height measurements to be confident in the quality of the data
+		const float hgt = gps.alt * 1e-3f;
+
+		if (_gps_hgt_counter == 0) {
+			_gps_hgt_lpf.reset(hgt);
+
+		} else {
+			_gps_hgt_lpf.update(hgt);
+		}
+
+		_gps_hgt_counter++;
+	}
 }
 
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
@@ -249,6 +273,17 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		_baro_alt_sum = 0.0f;
 		_baro_timestamp_sum = 0;
 	}
+
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_baro_hgt_counter == 0) {
+		_baro_hgt_lpf.reset(baro_sample.hgt);
+
+	} else {
+		_baro_hgt_lpf.update(baro_sample.hgt);
+	}
+
+	_baro_hgt_counter++;
 }
 
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
@@ -308,6 +343,8 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 
 		_range_buffer.push(range_sample_new);
 	}
+
+	_rng_hgt_filter.insert(range_sample.rng);
 }
 
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
@@ -369,6 +406,17 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 
 		_ext_vision_buffer.push(ev_sample_new);
 	}
+
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_ev_hgt_counter == 0) {
+		_ev_hgt_lpf.reset(evdata.pos(2));
+
+	} else {
+		_ev_hgt_lpf.update(evdata.pos(2));
+	}
+
+	_ev_hgt_counter++;
 }
 
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index fe04e5e293..514443bbe9 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -63,6 +63,7 @@
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
+#include <mathlib/math/filter/MedianFilter.hpp>
 
 using namespace estimator;
 
@@ -410,6 +411,21 @@ protected:
 	warning_event_status_u _warning_events{};
 	information_event_status_u _information_events{};
 
+	uint32_t _baro_hgt_counter{0};
+	AlphaFilter<float> _baro_hgt_lpf{0.1f};
+
+	uint32_t _gps_hgt_counter{0};
+	AlphaFilter<float> _gps_hgt_lpf{0.1f};
+
+	uint32_t _rng_hgt_counter{0};
+	math::MedianFilter<float, 5> _rng_hgt_filter{};
+
+	uint32_t _ev_hgt_counter{0};
+	AlphaFilter<float> _ev_hgt_lpf{0.1f};
+
+	uint32_t _mag_counter{0};
+	AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
+
 private:
 
 	inline void setDragData(const imuSample &imu);
diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp
index f6332958f4..ec6dea72b4 100644
--- a/src/modules/ekf2/EKF/height_fusion.cpp
+++ b/src/modules/ekf2/EKF/height_fusion.cpp
@@ -40,11 +40,30 @@
 
 void Ekf::fuseBaroHgt()
 {
+	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+
+	if (!PX4_ISFINITE(_baro_hgt_offset)) {
+		_baro_hgt_offset = _state.pos(2) + unbiased_baro;
+	}
+
+	if (!_control_status_prev.flags.baro_hgt && _control_status.flags.baro_hgt) {
+		ECL_INFO("Baro height offset %.3f", (double)_baro_hgt_offset);
+	}
+
+	// calculate a filtered offset between the baro origin and local NED origin if we are not
+	// using the baro as a height reference
+	if (!_control_status.flags.baro_hgt && (_delta_time_baro_us != 0)) {
+		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
+
+		// apply a 10 second first order low pass filter to baro offset
+		const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
+		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
+	}
+
 	Vector2f baro_hgt_innov_gate;
 	Vector3f baro_hgt_obs_var;
 
 	// vertical position innovation - baro measurement has opposite sign to earth z axis
-	const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
 	_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
 	// observation variance - user parameter defined
 	baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
@@ -68,30 +87,72 @@ void Ekf::fuseBaroHgt()
 	}
 
 	fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
-			     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
+			     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio, _control_status.flags.baro_hgt);
 }
 
 void Ekf::fuseGpsHgt()
 {
+	if (!PX4_ISFINITE(_gps_hgt_offset)) {
+		_gps_hgt_offset = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref;
+	}
+
+	if (!_control_status_prev.flags.gps_hgt && _control_status.flags.gps_hgt) {
+		ECL_INFO("GPS height offset %.3f, Altitude ref: %.3f", (double)_gps_hgt_offset, (double)_gps_alt_ref);
+	}
+
+	if (!_control_status.flags.gps_hgt && (_delta_time_gps_us != 0)) {
+		// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
+		const float local_time_step = math::constrain(1e-6f * _delta_time_gps_us, 0.f, 1.f);
+
+		// apply a 10 second first order low pass filter to height offset
+		_gps_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
+	}
+
 	Vector2f gps_hgt_innov_gate;
 	Vector3f gps_hgt_obs_var;
 	// vertical position innovation - gps measurement has opposite sign to earth z axis
-	_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+	_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _gps_hgt_offset;
 	gps_hgt_obs_var(2) = getGpsHeightVariance();
 	// innovation gate size
 	gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 
 	fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
-			     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+			     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio, _control_status.flags.gps_hgt);
 }
 
 void Ekf::fuseRngHgt()
 {
+	if (!PX4_ISFINITE(_rng_hgt_offset)) {
+		// calculate height sensor offset such that current measurement matches our current height estimate
+		// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
+		if (_control_status.flags.in_air && isTerrainEstimateValid()) {
+			_rng_hgt_offset = _terrain_vpos;
+
+		} else if (_control_status.flags.in_air) {
+			_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
+
+		} else {
+			_rng_hgt_offset = _params.rng_gnd_clearance;
+		}
+	}
+
+	if (!_control_status_prev.flags.rng_hgt && _control_status.flags.rng_hgt) {
+		ECL_INFO("RNG height offset %.3f", (double)_rng_hgt_offset);
+	}
+
+	if (!_control_status.flags.rng_hgt && (_delta_time_rng_us != 0)) {
+		// calculate a filtered offset between the RNG origin and local NED origin if we are not using the GPS as a height reference
+		const float local_time_step = math::constrain(1e-6f * _delta_time_rng_us, 0.f, 1.f);
+
+		// apply a 10 second first order low pass filter to height offset
+		_rng_hgt_offset += local_time_step * math::constrain(0.1f * _rng_hgt_innov(2), -0.1f, 0.1f);
+	}
+
 	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_sensor.getDistBottom(),
-					     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+					     _params.rng_gnd_clearance)) - _rng_hgt_offset;
 	// observation variance - user parameter defined
 	rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
 				   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
@@ -99,20 +160,37 @@ void Ekf::fuseRngHgt()
 	rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
 
 	fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
-			     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
+			     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio, _control_status.flags.rng_hgt);
 }
 
 void Ekf::fuseEvHgt()
 {
+	if (!PX4_ISFINITE(_ev_hgt_offset)) {
+		_ev_hgt_offset = _state.pos(2) - _ev_sample_delayed.pos(2);
+	}
+
+	if (!_control_status_prev.flags.ev_hgt && _control_status.flags.ev_hgt) {
+		ECL_INFO("Vision height offset %.3f", (double)_ev_hgt_offset);
+	}
+
+	if (!_control_status.flags.ev_hgt && (_delta_time_ev_us != 0)) {
+		// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
+		const float local_time_step = math::constrain(1e-6f * _delta_time_ev_us, 0.f, 1.f);
+
+		// apply a 10 second first order low pass filter to height offset
+		_ev_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
+	}
+
 	Vector2f ev_hgt_innov_gate;
 	Vector3f ev_hgt_obs_var;
 	// calculate the innovation assuming the external vision observation is in local NED frame
-	_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
+	_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2) - _ev_hgt_offset;
+
 	// observation variance - defined externally
 	ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
 	// innovation gate size
 	ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
 
 	fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
-			     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+			     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio, _control_status.flags.ev_hgt);
 }
diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp
index ea123805df..afc93308b2 100644
--- a/src/modules/ekf2/EKF/sensor_range_finder.hpp
+++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp
@@ -105,6 +105,8 @@ public:
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
+	const rangeSample &sample() const { return _sample; }
+
 private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
 
diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp
index 3df12d7b2e..62678ef203 100644
--- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp
+++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp
@@ -147,9 +147,8 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
 }
 
 bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
-			       Vector3f &innov_var, Vector2f &test_ratio)
+			       Vector3f &innov_var, Vector2f &test_ratio, bool fuse)
 {
-
 	innov_var(2) = P(9, 9) + obs_var(2);
 	test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
 	_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
@@ -170,16 +169,21 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
 	}
 
 	if (innov_check_pass) {
-		_time_last_hgt_fuse = _time_last_imu;
-		_innov_check_fail_status.flags.reject_ver_pos = false;
-		fuseVelPosHeight(innovation, innov_var(2), 5);
+		if (fuse) {
+			_time_last_hgt_fuse = _time_last_imu;
+			_innov_check_fail_status.flags.reject_ver_pos = false;
+			fuseVelPosHeight(innovation, innov_var(2), 5);
+		}
 
 		return true;
 
-	} else {
-		_innov_check_fail_status.flags.reject_ver_pos = true;
-		return false;
 	}
+
+	if (fuse) {
+		_innov_check_fail_status.flags.reject_ver_pos = true;
+	}
+
+	return false;
 }
 
 // Helper function that fuses a single velocity or position measurement