diff --git a/src/modules/ekf2/EKF/bias_estimator.cpp b/src/modules/ekf2/EKF/bias_estimator.cpp
index 389b3e9b18..545b969d16 100644
--- a/src/modules/ekf2/EKF/bias_estimator.cpp
+++ b/src/modules/ekf2/EKF/bias_estimator.cpp
@@ -97,7 +97,7 @@ inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) cons
 
 inline void BiasEstimator::updateState(const float K, const float innov)
 {
-	_state = math::constrain(_state + K * innov, -_state_max, _state_max);
+	_state = _state + K * innov;
 }
 
 inline void BiasEstimator::updateStateCovariance(const float K)
diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp
index 5f70ec5204..637efc14a8 100644
--- a/src/modules/ekf2/EKF/bias_estimator.hpp
+++ b/src/modules/ekf2/EKF/bias_estimator.hpp
@@ -64,9 +64,18 @@ public:
 		float innov_test_ratio;
 	};
 
-	BiasEstimator() = default;
+	BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {};
 	virtual ~BiasEstimator() = default;
 
+	void reset()
+	{
+		_state = 0.f;
+		_state_var = 0.f;
+		_signed_innov_test_ratio_lpf.reset(0.f);
+		_time_since_last_negative_innov = 0.f;
+		_time_since_last_positive_innov = 0.f;
+	}
+
 	virtual void predict(float dt);
 	virtual void fuseBias(float measurement, float measurement_var);
 
@@ -86,7 +95,6 @@ public:
 
 private:
 	float _state{0.f};
-	float _state_max{100.f};
 	float _dt{0.01f};
 
 	float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio)
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 2387dc5a15..36370606bc 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -105,12 +105,12 @@ enum TerrainFusionMask : uint8_t {
 	TerrainFuseOpticalFlow = (1 << 1)
 };
 
-enum VerticalHeightSensor : uint8_t {
-	// Integer definitions for vdist_sensor_type
-	BARO  = 0,   	///< Use baro height
-	GPS   = 1,   	///< Use GPS height
-	RANGE = 2,   	///< Use range finder height
-	EV    = 3    	///< Use external vision
+enum HeightSensor : uint8_t {
+	BARO  = 0,
+	GNSS  = 1,
+	RANGE = 2,
+	EV    = 3,
+	UNKNOWN  = 4
 };
 
 enum SensorFusionMask : uint16_t {
@@ -123,7 +123,11 @@ enum SensorFusionMask : uint16_t {
 	USE_DRAG         = (1<<5),      ///< set to true to use the multi-rotor drag model to estimate wind
 	ROTATE_EXT_VIS   = (1<<6),      ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
 	USE_GPS_YAW      = (1<<7),      ///< set to true to use GPS yaw data if available
-	USE_EXT_VIS_VEL  = (1<<8)       ///< set to true to use external vision velocity data
+	USE_EXT_VIS_VEL  = (1<<8),      ///< set to true to use external vision velocity data
+	USE_BARO_HGT     = (1<<9),      ///< set to true to use baro data
+	USE_GPS_HGT      = (1<<10),     ///< set to true to use GPS height data
+	USE_RNG_HGT      = (1<<11),     ///< set to true to use range height data
+	USE_EXT_VIS_HGT  = (1<<12)     ///< set to true to use external vision height data
 };
 
 struct gpsMessage {
@@ -247,8 +251,9 @@ struct parameters {
 	int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
 
 	// measurement source control
-	int32_t fusion_mode{SensorFusionMask::USE_GPS};         ///< bitmasked integer that selects which aiding sources will be used
-	int32_t vdist_sensor_type{VerticalHeightSensor::BARO};  ///< selects the primary source for height data
+	int32_t fusion_mode{SensorFusionMask::USE_GPS |
+			    SensorFusionMask::USE_BARO_HGT};         ///< bitmasked integer that selects which aiding sources will be used
+	int32_t height_sensor_ref{HeightSensor::BARO};
 	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
 				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
 
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index 68e4f61267..ac7e73dc84 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -172,9 +172,6 @@ void Ekf::controlFusionModes()
 	// run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available
 	runYawEKFGSF();
 
-	// check for height sensor timeouts and reset and change sensor if necessary
-	controlHeightSensorTimeouts();
-
 	// control use of observations for aiding
 	controlMagFusion();
 	controlOpticalFlowFusion();
@@ -660,134 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 	}
 }
 
-void Ekf::controlHeightSensorTimeouts()
-{
-	/*
-	 * Handle the case where we have not fused height measurements recently and
-	 * uncertainty exceeds the max allowable. Reset using the best available height
-	 * measurement source, continue using it after the reset and declare the current
-	 * source failed if we have switched.
-	*/
-
-	checkVerticalAccelerationHealth();
-
-	// check if height is continuously failing because of accel errors
-	const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
-
-	// check if height has been inertial deadreckoning for too long
-	// in vision hgt mode check for vision data
-	const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
-
-	if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
-
-		const char *failing_height_source = nullptr;
-		const char *new_height_source = nullptr;
-
-		if (_control_status.flags.baro_hgt) {
-			bool reset_to_gps = false;
-
-			// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
-			if (!_gps_intermittent) {
-				reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent;
-			}
-
-			if (reset_to_gps) {
-				// set height sensor health
-				_baro_hgt_faulty = true;
-
-				startGpsHgtFusion();
-
-				failing_height_source = "baro";
-				new_height_source = "gps";
-
-			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
-				resetHeightToBaro();
-
-				failing_height_source = "baro";
-				new_height_source = "baro";
-			}
-
-		} else if (_control_status.flags.gps_hgt) {
-			bool reset_to_baro = false;
-
-			// if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro
-			if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
-				reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent;
-			}
-
-			if (reset_to_baro) {
-				startBaroHgtFusion();
-
-				failing_height_source = "gps";
-				new_height_source = "baro";
-
-			} else if (!_gps_intermittent) {
-				resetHeightToGps();
-
-				failing_height_source = "gps";
-				new_height_source = "gps";
-			}
-
-		} else if (_control_status.flags.rng_hgt) {
-
-			if (_range_sensor.isHealthy()) {
-				resetHeightToRng();
-
-				failing_height_source = "rng";
-				new_height_source = "rng";
-
-			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
-				startBaroHgtFusion();
-
-				failing_height_source = "rng";
-				new_height_source = "baro";
-			}
-
-		} else if (_control_status.flags.ev_hgt) {
-			// check if vision data is available
-			bool ev_data_available = false;
-
-			if (_ext_vision_buffer) {
-				const extVisionSample &ev_init = _ext_vision_buffer->get_newest();
-				ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
-			}
-
-			if (ev_data_available) {
-				resetHeightToEv();
-
-				failing_height_source = "ev";
-				new_height_source = "ev";
-
-			} else if (_range_sensor.isHealthy()) {
-				// Fallback to rangefinder data if available
-				startRngHgtFusion();
-
-				failing_height_source = "ev";
-				new_height_source = "rng";
-
-			} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
-				startBaroHgtFusion();
-
-				failing_height_source = "ev";
-				new_height_source = "baro";
-			}
-		}
-
-		if (failing_height_source && new_height_source) {
-			_warning_events.flags.height_sensor_timeout = true;
-			ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
-		}
-
-		// Also reset the vertical velocity
-		if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) {
-			resetVerticalVelocityToGps(_gps_sample_delayed);
-
-		} else {
-			resetVerticalVelocityToZero();
-		}
-	}
-}
-
 void Ekf::checkVerticalAccelerationHealth()
 {
 	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
@@ -797,6 +666,7 @@ void Ekf::checkVerticalAccelerationHealth()
 	// Don't use stale innovation data.
 	bool is_inertial_nav_falling = false;
 	bool are_vertical_pos_and_vel_independant = false;
+	bool is_using_multiple_hgt_sources = false;
 
 	if (_control_status.flags.gps) {
 		// GNSS velocity
@@ -827,6 +697,8 @@ void Ekf::checkVerticalAccelerationHealth()
 	}
 
 	if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
+		is_using_multiple_hgt_sources = _control_status.flags.gps_hgt && _control_status.flags.baro_hgt;
+
 		if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
 			// If vertical position and velocity come from independent sensors then we can
 			// trust them more if they disagree with the IMU, but need to check that they agree
@@ -861,7 +733,7 @@ void Ekf::checkVerticalAccelerationHealth()
 
 	// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
 	// innovations are large
-	const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
+	const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling;
 
 	if (bad_vert_accel) {
 		_time_bad_vert_accel = _time_last_imu;
@@ -882,115 +754,75 @@ void Ekf::checkVerticalAccelerationHealth()
 
 void Ekf::controlHeightFusion()
 {
+	checkVerticalAccelerationHealth();
 	checkRangeAidSuitability();
-	const bool do_range_aid = (_params.range_aid == 1) && _is_range_aid_suitable;
 
-	switch (_params.vdist_sensor_type) {
-	default:
-		ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
-
-	// FALLTHROUGH
-	case VerticalHeightSensor::BARO:
-		if (do_range_aid) {
-			if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-				startRngAidHgtFusion();
-			}
-
-		} else {
-			if (!_control_status.flags.baro_hgt) {
-				if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
-					startBaroHgtFusion();
-
-				} else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) {
-					// Use GPS as a fallback
-					startGpsHgtFusion();
-				}
-			}
-		}
-
-		break;
-
-	case VerticalHeightSensor::RANGE:
-
-		// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
-		// and are on the ground, then synthesise a measurement at the expected on ground value
-		if (!_control_status.flags.in_air
-		    && !_range_sensor.isDataHealthy()
-		    && _range_sensor.isRegularlySendingData()
-		    && _range_sensor.isDataReady()) {
-
-			_range_sensor.setRange(_params.rng_gnd_clearance);
-			_range_sensor.setValidity(true); // bypass the checks
-		}
-
-		if (!_control_status.flags.rng_hgt) {
-			if (_range_sensor.isDataHealthy()) {
-				startRngHgtFusion();
-			}
-		}
-
-		break;
-
-	case VerticalHeightSensor::GPS:
-
-		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
-		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
-		// Do switching between GPS and rangefinder if using range finder as a height source when close
-		// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
-		if (do_range_aid) {
-			if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-				startRngAidHgtFusion();
-			}
-
-		} else {
-			if (!_control_status.flags.gps_hgt) {
-				if (!_gps_intermittent && _gps_checks_passed) {
-					// In fallback mode and GPS has recovered so start using it
-					startGpsHgtFusion();
-
-				} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) {
-					// Use baro as a fallback
-					startBaroHgtFusion();
-				}
-			}
-		}
-
-		break;
-
-	case VerticalHeightSensor::EV:
-
-		// don't start using EV data unless data is arriving frequently
-		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-			startEvHgtFusion();
-		}
-
-		break;
-	}
-
-	updateBaroHgtBias();
-	updateBaroHgtOffset();
 	updateGroundEffect();
 
-	if (_baro_data_ready) {
-		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
+	controlBaroHeightFusion();
+	controlGnssHeightFusion();
+	controlRangeHeightFusion();
+	controlEvHeightFusion();
 
-		if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
-			fuseBaroHgt(_aid_src_baro_hgt);
-		}
+	checkHeightSensorRefFallback();
+}
+
+void Ekf::checkHeightSensorRefFallback()
+{
+	if (_height_sensor_ref != HeightSensor::UNKNOWN) {
+		// The reference sensor is running, all good
+		return;
 	}
 
-	if (_rng_data_ready) {
-		updateRngHgt(_aid_src_rng_hgt);
+	HeightSensor fallback_list[4];
 
-		if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-			fuseRngHgt(_aid_src_rng_hgt);
-		}
+	switch (_params.height_sensor_ref) {
+	default:
+	/* FALLTHROUGH */
+	case HeightSensor::UNKNOWN:
+		fallback_list[0] = HeightSensor::GNSS;
+		fallback_list[1] = HeightSensor::BARO;
+		fallback_list[2] = HeightSensor::EV;
+		fallback_list[3] = HeightSensor::RANGE;
+		break;
+
+	case HeightSensor::BARO:
+		fallback_list[0] = HeightSensor::BARO;
+		fallback_list[1] = HeightSensor::GNSS;
+		fallback_list[2] = HeightSensor::EV;
+		fallback_list[3] = HeightSensor::RANGE;
+		break;
+
+	case HeightSensor::GNSS:
+		fallback_list[0] = HeightSensor::GNSS;
+		fallback_list[1] = HeightSensor::BARO;
+		fallback_list[2] = HeightSensor::EV;
+		fallback_list[3] = HeightSensor::RANGE;
+		break;
+
+	case HeightSensor::RANGE:
+		fallback_list[0] = HeightSensor::RANGE;
+		fallback_list[1] = HeightSensor::EV;
+		fallback_list[2] = HeightSensor::BARO;
+		fallback_list[3] = HeightSensor::GNSS;
+		break;
+
+	case HeightSensor::EV:
+		fallback_list[0] = HeightSensor::EV;
+		fallback_list[1] = HeightSensor::RANGE;
+		fallback_list[2] = HeightSensor::BARO;
+		fallback_list[3] = HeightSensor::GNSS;
+		break;
 	}
 
-	if (_control_status.flags.ev_hgt) {
-
-		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-			fuseEvHgt();
+	for (unsigned i = 0; i < 4; i++) {
+		if (((fallback_list[i] == HeightSensor::BARO) && _control_status.flags.baro_hgt)
+		    || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
+		    || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
+		    || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
+			ECL_INFO("fallback to secondary height reference");
+			_height_sensor_ref = fallback_list[i];
+			break;
 		}
 	}
 }
@@ -1024,6 +856,199 @@ void Ekf::checkRangeAidSuitability()
 	}
 }
 
+void Ekf::controlBaroHeightFusion()
+{
+	if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) {
+		stopBaroHgtFusion();
+		return;
+	}
+
+	_baro_b_est.predict(_dt_ekf_avg);
+
+	if (_baro_data_ready) {
+		_baro_lpf.update(_baro_sample_delayed.hgt);
+		updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
+
+		const bool continuing_conditions_passing = !_baro_hgt_faulty && !_baro_hgt_intermittent;
+		const bool starting_conditions_passing = continuing_conditions_passing;
+
+		if (_control_status.flags.baro_hgt) {
+			if (continuing_conditions_passing) {
+				fuseBaroHgt(_aid_src_baro_hgt);
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, (uint64_t)5e6);
+
+				if (isHeightResetRequired()) {
+					// All height sources are failing
+					resetHeightToBaro();
+					resetVerticalVelocityToZero();
+
+				} else if (is_fusion_failing) {
+					// Some other height source is still working
+					stopBaroHgtFusion();
+					_baro_hgt_faulty = true;
+				}
+
+			} else {
+				stopBaroHgtFusion();
+			}
+		} else {
+			if (starting_conditions_passing) {
+				startBaroHgtFusion();
+			}
+		}
+
+	} else if (_control_status.flags.baro_hgt && _baro_hgt_intermittent) {
+		// No baro data anymore. Stop until it comes back.
+		stopBaroHgtFusion();
+	}
+}
+
+void Ekf::controlGnssHeightFusion()
+{
+	if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) {
+		stopGpsHgtFusion();
+		return;
+	}
+
+	_gps_hgt_b_est.predict(_dt_ekf_avg);
+
+	if (_gps_data_ready) {
+		const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised;
+		const bool starting_conditions_passing = continuing_conditions_passing;
+
+		if (_control_status.flags.gps_hgt) {
+			if (continuing_conditions_passing) {
+				/* fuseGpsHgt(); */ // Done in fuseGpsPos
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.reset_timeout_max);
+
+				if (isHeightResetRequired()) {
+					// All height sources are failing
+					resetHeightToGps();
+					resetVerticalVelocityToGps(_gps_sample_delayed);
+
+				} else if (is_fusion_failing) {
+					// Some other height source is still working
+					stopGpsHgtFusion();
+				}
+
+			} else {
+				stopGpsHgtFusion();
+			}
+		} else {
+			if (starting_conditions_passing) {
+				startGpsHgtFusion();
+			}
+		}
+
+	} else if (_control_status.flags.gps_hgt && _gps_intermittent) {
+		stopGpsHgtFusion();
+	}
+}
+
+void Ekf::controlRangeHeightFusion()
+{
+	if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
+		stopRngHgtFusion();
+		return;
+	}
+
+	_rng_hgt_b_est.predict(_dt_ekf_avg);
+
+	// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
+	// and are on the ground, then synthesise a measurement at the expected on ground value
+	if (!_control_status.flags.in_air
+	    && !_range_sensor.isDataHealthy()
+	    && _range_sensor.isRegularlySendingData()
+	    && _range_sensor.isDataReady()) {
+
+		_range_sensor.setRange(_params.rng_gnd_clearance);
+		_range_sensor.setValidity(true); // bypass the checks
+	}
+
+	if (_rng_data_ready) {
+
+		// TODO: primary height source:
+		// run a bias estimator for all other sources
+		updateRngHgt(_aid_src_rng_hgt);
+
+		const bool continuing_conditions_passing = _range_sensor.isDataHealthy();
+		const bool starting_conditions_passing = continuing_conditions_passing
+							 && _range_sensor.isRegularlySendingData();
+
+		if (_control_status.flags.rng_hgt) {
+			if (continuing_conditions_passing) {
+				fuseRngHgt(_aid_src_rng_hgt);
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6);
+
+				if (isHeightResetRequired()) {
+					// All height sources are failing
+					resetHeightToRng();
+					resetVerticalVelocityToZero();
+
+				} else if (is_fusion_failing) {
+					// Some other height source is still working
+					stopRngHgtFusion();
+					_control_status.flags.rng_fault = true;
+					_range_sensor.setFaulty();
+				}
+
+			} else {
+				stopRngHgtFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				startRngHgtFusion();
+			}
+		}
+
+	} else if (_control_status.flags.rng_hgt && isTimedOut(_time_last_range, (uint64_t)5e6)) {
+		stopRngHgtFusion();
+	}
+}
+
+void Ekf::controlEvHeightFusion()
+{
+	if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) {
+		stopEvHgtFusion();
+		return;
+	}
+
+	_ev_hgt_b_est.predict(_dt_ekf_avg);
+
+	if (_ev_data_ready) {
+		const bool continuing_conditions_passing = true;
+		const bool starting_conditions_passing = continuing_conditions_passing
+							 && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL);
+
+		if (_control_status.flags.ev_hgt) {
+			if (continuing_conditions_passing) {
+				fuseEvHgt();
+
+				if (isHeightResetRequired()) {
+					// All height sources are failing
+					resetHeightToRng();
+					resetVerticalVelocityToZero();
+				}
+
+			} else {
+				stopEvHgtFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				startEvHgtFusion();
+			}
+		}
+
+	} else if (_control_status.flags.ev_hgt && isTimedOut(_time_last_ext_vision, (uint64_t)5e6)) {
+		stopEvHgtFusion();
+	}
+}
+
 void Ekf::controlAirDataFusion()
 {
 	// control activation and initialisation/reset of wind states required for airspeed fusion
diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp
index 2aaa41ebc6..15faaf1ad1 100644
--- a/src/modules/ekf2/EKF/ekf.cpp
+++ b/src/modules/ekf2/EKF/ekf.cpp
@@ -164,10 +164,12 @@ bool Ekf::initialiseFilter()
 	if (_baro_buffer && _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;
+				_baro_lpf.reset(_baro_sample_delayed.hgt);
 
 			} else {
-				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+				_baro_lpf.update(_baro_sample_delayed.hgt);
+
+				_baro_b_est.setBias(_baro_lpf.getState());
 			}
 
 			_baro_counter++;
@@ -187,7 +189,7 @@ bool Ekf::initialiseFilter()
 	}
 
 	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
-	setControlBaroHeight();
+	_control_status.flags.baro_hgt = false;
 
 	if (!initialiseTilt()) {
 		return false;
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 8898d98eba..0c311ed785 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -47,6 +47,7 @@
 
 #include "EKFGSF_yaw.h"
 #include "bias_estimator.hpp"
+#include "height_bias_estimator.hpp"
 
 #include <uORB/topics/estimator_aid_source_1d.h>
 #include <uORB/topics/estimator_aid_source_2d.h>
@@ -586,14 +587,7 @@ private:
 
 	// 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 _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
-	float _gps_hgt_offset{0.0f};		///< GPS height reading at the local NED origin (m)
-	float _rng_hgt_offset{0.0f};		///< Range height reading at the local NED origin (m)
-	float _ev_hgt_offset{0.0f};		///< EV height reading at the local NED origin (m)
-
-	float _baro_hgt_bias{0.0f};
-	float _baro_hgt_bias_var{1.f};
+	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
 
 	// Variables used to control activation of post takeoff functionality
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
@@ -709,6 +703,8 @@ private:
 	void resetHorizontalPositionToLastKnown();
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos);
 
+	bool isHeightResetRequired() const;
+
 	void resetVerticalPositionTo(float new_vert_pos);
 
 	void resetHeightToBaro();
@@ -923,9 +919,6 @@ private:
 	void runMagAndMagDeclFusions(const Vector3f &mag);
 	void run3DMagAndDeclFusions(const Vector3f &mag);
 
-	// control fusion of range finder observations
-	void controlRangeFinderFusion();
-
 	// control fusion of air data observations
 	void controlAirDataFusion();
 
@@ -935,9 +928,6 @@ private:
 	// control fusion of multi-rotor drag specific force observations
 	void controlDragFusion();
 
-	// control fusion of pressure altitude observations
-	void controlBaroFusion();
-
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
@@ -948,29 +938,19 @@ private:
 	// control fusion of auxiliary velocity observations
 	void controlAuxVelFusion();
 
-	// control for height sensor timeouts, sensor changes and state resets
-	void controlHeightSensorTimeouts();
-
 	void checkVerticalAccelerationHealth();
 
 	// control for combined height fusion mode (implemented for switching between baro and range height)
 	void controlHeightFusion();
+	void checkHeightSensorRefFallback();
+	void controlBaroHeightFusion();
+	void controlGnssHeightFusion();
+	void controlRangeHeightFusion();
+	void controlEvHeightFusion();
 
 	// determine if flight condition is suitable to use range finder instead of the primary height sensor
 	void checkRangeAidSuitability();
 
-	// set control flags to use baro height
-	void setControlBaroHeight();
-
-	// set control flags to use range height
-	void setControlRangeHeight();
-
-	// set control flags to use GPS height
-	void setControlGPSHeight();
-
-	// set control flags to use external vision height
-	void setControlEVHeight();
-
 	void stopMagFusion();
 	void stopMag3DFusion();
 	void stopMagHdgFusion();
@@ -978,13 +958,16 @@ private:
 	void startMag3DFusion();
 
 	void startBaroHgtFusion();
+	void stopBaroHgtFusion();
+
 	void startGpsHgtFusion();
+	void stopGpsHgtFusion();
+
 	void startRngHgtFusion();
+	void stopRngHgtFusion();
 	void startRngAidHgtFusion();
 	void startEvHgtFusion();
-
-	void updateBaroHgtOffset();
-	void updateBaroHgtBias();
+	void stopEvHgtFusion();
 
 	void updateGroundEffect();
 
@@ -1091,7 +1074,12 @@ private:
 	// yaw estimator instance
 	EKFGSF_yaw _yawEstimator{};
 
-	BiasEstimator _baro_b_est{};
+	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
+
+	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
+	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
+	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
+	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 95461113fd..906bebe35d 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -213,6 +213,18 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos)
 	_time_last_hor_pos_fuse = _time_last_imu;
 }
 
+bool Ekf::isHeightResetRequired() const
+{
+	// check if height is continuously failing because of accel errors
+	const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
+
+	// check if height has been inertial deadreckoning for too long
+	const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
+
+	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
+}
+
+
 void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 {
 	const float old_vert_pos = _state.pos(2);
@@ -244,10 +256,14 @@ void Ekf::resetHeightToBaro()
 	ECL_INFO("reset height to baro");
 	_information_events.flags.reset_hgt_to_baro = true;
 
-	resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_hgt_offset));
+	resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_b_est.getBias()));
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
+
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
 }
 
 void Ekf::resetHeightToGps()
@@ -255,14 +271,16 @@ void Ekf::resetHeightToGps()
 	ECL_INFO("reset height to GPS");
 	_information_events.flags.reset_hgt_to_gps = true;
 
-	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-_gps_sample_delayed.hgt + getEkfGlobalOriginAltitude());
+	resetVerticalPositionTo(-(_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias()));
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance());
 
-	// adjust the baro offset
-	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+	_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
+
+	_aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu;
 }
 
 void Ekf::resetHeightToRng()
@@ -281,14 +299,14 @@ void Ekf::resetHeightToRng()
 	}
 
 	// update the state and associated variance
-	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset);
+	resetVerticalPositionTo(-(dist_bottom - _rng_hgt_b_est.getBias()));
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
 
-	// adjust the baro offset
-	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+	_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
 }
 
 void Ekf::resetHeightToEv()
@@ -296,14 +314,14 @@ void Ekf::resetHeightToEv()
 	ECL_INFO("reset height to EV");
 	_information_events.flags.reset_hgt_to_ev = true;
 
-	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(_ev_sample_delayed.pos(2));
+	resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias());
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
 
-	// adjust the baro offset
-	_baro_hgt_offset += _state.pos(2) - z_pos_before_reset;
+	_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
+	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
+	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
 }
 
 void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed)
@@ -733,10 +751,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
 
-			_baro_hgt_offset += _state_reset_status.posD_change;
-
-			_rng_hgt_offset += _state_reset_status.posD_change;
-			_ev_hgt_offset -= _state_reset_status.posD_change;
+			_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
+			_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
+			_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
 		}
 
 		return true;
@@ -1232,42 +1249,6 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 	}
 }
 
-void Ekf::setControlBaroHeight()
-{
-	_control_status.flags.baro_hgt = true;
-
-	_control_status.flags.gps_hgt = false;
-	_control_status.flags.rng_hgt = false;
-	_control_status.flags.ev_hgt = false;
-}
-
-void Ekf::setControlRangeHeight()
-{
-	_control_status.flags.rng_hgt = true;
-
-	_control_status.flags.baro_hgt = false;
-	_control_status.flags.gps_hgt = false;
-	_control_status.flags.ev_hgt = false;
-}
-
-void Ekf::setControlGPSHeight()
-{
-	_control_status.flags.gps_hgt = true;
-
-	_control_status.flags.baro_hgt = false;
-	_control_status.flags.rng_hgt = false;
-	_control_status.flags.ev_hgt = false;
-}
-
-void Ekf::setControlEVHeight()
-{
-	_control_status.flags.ev_hgt = true;
-
-	_control_status.flags.baro_hgt = false;
-	_control_status.flags.gps_hgt = false;
-	_control_status.flags.rng_hgt = false;
-}
-
 void Ekf::stopMagFusion()
 {
 	stopMag3DFusion();
@@ -1325,96 +1306,130 @@ void Ekf::startMag3DFusion()
 void Ekf::startBaroHgtFusion()
 {
 	if (!_control_status.flags.baro_hgt) {
-		if (!_control_status.flags.rng_hgt) {
+		if (_params.height_sensor_ref == HeightSensor::BARO) {
+			_height_sensor_ref = HeightSensor::BARO;
 			resetHeightToBaro();
+
+		} else {
+			_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
 		}
 
-		setControlBaroHeight();
-
-		// We don't need to set a height sensor offset
-		// since we track a separate _baro_hgt_offset
-
+		_control_status.flags.baro_hgt = true;
+		_baro_b_est.setFusionActive();
 		ECL_INFO("starting baro height fusion");
 	}
 }
 
+void Ekf::stopBaroHgtFusion()
+{
+	if (_control_status.flags.baro_hgt) {
+		if (_height_sensor_ref == HeightSensor::BARO) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
+
+		_control_status.flags.baro_hgt = false;
+		_baro_b_est.setFusionInactive();
+		ECL_INFO("stopping baro height fusion");
+	}
+}
+
 void Ekf::startGpsHgtFusion()
 {
 	if (!_control_status.flags.gps_hgt) {
-		if (_control_status.flags.rng_hgt) {
-			// switch out of range aid
-			// calculate height sensor offset such that current
-			// measurement matches our current height estimate
-			_gps_hgt_offset = _gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() + _state.pos(2);
+
+		if (_params.height_sensor_ref == HeightSensor::GNSS) {
+			_gps_hgt_b_est.reset();
+			_height_sensor_ref = HeightSensor::GNSS;
+			resetHeightToGps();
 
 		} else {
-			_gps_hgt_offset = 0.f;
-			resetHeightToGps();
+			_gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude()));
+
+			// Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout
+			_aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu;
 		}
 
-		setControlGPSHeight();
-
+		_control_status.flags.gps_hgt = true;
+		_gps_hgt_b_est.setFusionActive();
 		ECL_INFO("starting GPS height fusion");
 	}
 }
 
+void Ekf::stopGpsHgtFusion()
+{
+	if (_control_status.flags.gps_hgt) {
+
+		if (_height_sensor_ref == HeightSensor::GNSS) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
+
+		_control_status.flags.gps_hgt = false;
+		_gps_hgt_b_est.setFusionInactive();
+		ECL_INFO("stopping GPS height fusion");
+	}
+}
+
 void Ekf::startRngHgtFusion()
 {
 	if (!_control_status.flags.rng_hgt) {
-		setControlRangeHeight();
-
-		// Range finder is the primary height source, the ground is now the datum used
-		// to compute the local vertical position
-		_rng_hgt_offset = 0.f;
-
-		if (!_control_status_prev.flags.ev_hgt) {
-			// EV and range finders are using the same height datum
+		if (_params.height_sensor_ref == HeightSensor::RANGE) {
+			// Range finder is the primary height source, the ground is now the datum used
+			// to compute the local vertical position
+			_rng_hgt_b_est.reset();
+			_height_sensor_ref = HeightSensor::RANGE;
 			resetHeightToRng();
+
+		} else {
+			_rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom());
 		}
 
+		_control_status.flags.rng_hgt = true;
+		_rng_hgt_b_est.setFusionActive();
 		ECL_INFO("starting RNG height fusion");
 	}
 }
 
-void Ekf::startRngAidHgtFusion()
+void Ekf::stopRngHgtFusion()
 {
-	if (!_control_status.flags.rng_hgt) {
-		setControlRangeHeight();
+	if (_control_status.flags.rng_hgt) {
+		if (_height_sensor_ref == HeightSensor::RANGE) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
 
-		// calculate height sensor offset such that current
-		// measurement matches our current height estimate
-		_rng_hgt_offset = _terrain_vpos;
-
-		ECL_INFO("starting RNG aid height fusion");
+		_control_status.flags.rng_hgt = false;
+		_rng_hgt_b_est.setFusionInactive();
+		ECL_INFO("stopping range height fusion");
 	}
 }
 
 void Ekf::startEvHgtFusion()
 {
 	if (!_control_status.flags.ev_hgt) {
-		setControlEVHeight();
-
-		if (!_control_status_prev.flags.rng_hgt) {
-			// EV and range finders are using the same height datum
+		if (_params.height_sensor_ref == HeightSensor::EV) {
+			_rng_hgt_b_est.reset();
+			_height_sensor_ref = HeightSensor::EV;
 			resetHeightToEv();
+
+		} else {
+			_ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2));
 		}
 
+		_control_status.flags.ev_hgt = true;
+		_ev_hgt_b_est.setFusionActive();
 		ECL_INFO("starting EV height fusion");
 	}
 }
 
-void Ekf::updateBaroHgtOffset()
+void Ekf::stopEvHgtFusion()
 {
-	// 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 && (_delta_time_baro_us != 0)) {
-		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
+	if (_control_status.flags.ev_hgt) {
+		if (_height_sensor_ref == HeightSensor::EV) {
+			_height_sensor_ref = HeightSensor::UNKNOWN;
+		}
 
-		// 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);
+		_control_status.flags.ev_hgt = false;
+		_ev_hgt_b_est.setFusionInactive();
+		ECL_INFO("stopping EV height fusion");
 	}
 }
 
@@ -1428,27 +1443,6 @@ float Ekf::getGpsHeightVariance()
 	return gps_alt_var;
 }
 
-void Ekf::updateBaroHgtBias()
-{
-	// Baro bias estimation using GPS altitude
-	if (_baro_data_ready && (_delta_time_baro_us != 0)) {
-		const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
-		_baro_b_est.setMaxStateNoise(_params.baro_noise);
-		_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
-		_baro_b_est.predict(dt);
-	}
-
-	if (_gps_data_ready && !_gps_intermittent
-	    && _gps_checks_passed && _NED_origin_initialised
-	    && !_baro_hgt_faulty) {
-		// Use GPS altitude as a reference to compute the baro bias measurement
-		const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
-					- (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude());
-		const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise);
-		_baro_b_est.fuseBias(baro_bias, baro_bias_var);
-	}
-}
-
 float Ekf::getRngHeightVariance() const
 {
 	const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
@@ -1660,14 +1654,13 @@ void Ekf::stopGpsFusion()
 
 void Ekf::stopGpsPosFusion()
 {
-	ECL_INFO("stopping GPS position fusion");
+	if (_control_status.flags.gps) {
+		ECL_INFO("stopping GPS position fusion");
+		_control_status.flags.gps = false;
+		stopGpsHgtFusion();
 
-	if (_control_status.flags.gps_hgt) {
-		ECL_INFO("stopping GPS height fusion");
-		startBaroHgtFusion();
+		resetEstimatorAidStatus(_aid_src_gnss_pos);
 	}
-
-	resetEstimatorAidStatus(_aid_src_gnss_pos);
 }
 
 void Ekf::stopGpsVelFusion()
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
index 326390cfae..7547e9d906 100644
--- a/src/modules/ekf2/EKF/estimator_interface.cpp
+++ b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -437,7 +437,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
 
 	// using baro
-	if (_params.vdist_sensor_type == 0) {
+	if (_params.fusion_mode & SensorFusionMask::USE_BARO_HGT) {
 		max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
 	}
 
@@ -452,7 +452,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 
 	// range aid or range height
-	if (_params.range_aid || (_params.vdist_sensor_type == VerticalHeightSensor::RANGE)) {
+	if (_params.range_aid || (_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
 
diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp
index 19828df5dc..33538096b0 100644
--- a/src/modules/ekf2/EKF/gps_fusion.cpp
+++ b/src/modules/ekf2/EKF/gps_fusion.cpp
@@ -102,12 +102,23 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	auto &gps_pos = _aid_src_gnss_pos;
 	resetEstimatorAidStatus(gps_pos);
 
+	const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
+	const float height_measurement_var = getGpsHeightVariance();
+
+	// save current bias and update bias estimator
+	const float bias = _gps_hgt_b_est.getBias();
+	const float bias_var = _gps_hgt_b_est.getBiasVar();
+
+	_gps_hgt_b_est.setMaxStateNoise(height_measurement_var);
+	_gps_hgt_b_est.setProcessNoiseStdDev(height_measurement_var); //TODO: update this
+	_gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9));
+
 	Vector3f position;
 	position(0) = gps_sample.pos(0);
 	position(1) = gps_sample.pos(1);
 
 	// vertical position - gps measurement has opposite sign to earth z axis
-	position(2) = -(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_offset);
+	position(2) = -(height_measurement - bias);
 
 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 
@@ -125,7 +136,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 		obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
 	}
 
-	obs_var(2) = getGpsHeightVariance();
+	obs_var(2) = height_measurement_var + bias_var;
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp
index 5c80466e31..40dcfc8e99 100644
--- a/src/modules/ekf2/EKF/height_fusion.cpp
+++ b/src/modules/ekf2/EKF/height_fusion.cpp
@@ -44,17 +44,18 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
 	resetEstimatorAidStatusFlags(baro_hgt);
 
 	// innovation gate size
-	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
+	const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
 
-	// observation variance - user parameter defined
-	float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	// measurement variance - user parameter defined
+	const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f));
+	const float measurement = _baro_sample_delayed.hgt;
 
 	// vertical position innovation - baro measurement has opposite sign to earth z axis
-	baro_hgt.observation = -(_baro_sample_delayed.hgt - _baro_b_est.getBias() - _baro_hgt_offset);
-	baro_hgt.observation_variance = obs_var;
+	baro_hgt.observation = -(measurement - _baro_b_est.getBias());
+	baro_hgt.observation_variance = measurement_var + _baro_b_est.getBiasVar();
 
 	baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
-	baro_hgt.innovation_variance = P(9, 9) + obs_var;
+	baro_hgt.innovation_variance = P(9, 9) + baro_hgt.observation_variance;
 
 	// Compensate for positive static pressure transients (negative vertical position innovations)
 	// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -86,6 +87,12 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
 	baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
 
 	baro_hgt.timestamp_sample = baro_sample.time_us;
+
+	// update the bias estimator before updating the main filter but after
+	// using its current state to compute the vertical position innovation
+	_baro_b_est.setMaxStateNoise(_params.baro_noise);
+	_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
+	_baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
 }
 
 void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
@@ -104,18 +111,19 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
 	// reset flags
 	resetEstimatorAidStatusFlags(rng_hgt);
 
-	// observation variance - user parameter defined
-	float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
+	// measurement variance - user parameter defined
+	const float measurement_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
 
 	// innovation gate size
-	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
+	const float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
 
 	// vertical position innovation, use range finder with tilt correction
-	rng_hgt.observation = (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) + _rng_hgt_offset;
-	rng_hgt.observation_variance = obs_var;
+	rng_hgt.observation = -(measurement - _rng_hgt_b_est.getBias());
+	rng_hgt.observation_variance = measurement_var + _rng_hgt_b_est.getBiasVar();
 
 	rng_hgt.innovation = _state.pos(2) - rng_hgt.observation;
-	rng_hgt.innovation_variance = P(9, 9) + obs_var;
+	rng_hgt.innovation_variance = P(9, 9) + rng_hgt.observation_variance;
 
 	setEstimatorAidStatusTestRatio(rng_hgt, innov_gate);
 
@@ -130,6 +138,14 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
 	rng_hgt.fusion_enabled = _control_status.flags.rng_hgt;
 
 	rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us;
+
+	// update the bias estimator before updating the main filter but after
+	// using its current state to compute the vertical position innovation
+	const float rng_noise = sqrtf(measurement_var);
+	_rng_hgt_b_est.setMaxStateNoise(rng_noise);
+	_rng_hgt_b_est.setProcessNoiseStdDev(rng_noise); // TODO: fix
+
+	_rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
 }
 
 void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
@@ -145,15 +161,25 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
 
 void Ekf::fuseEvHgt()
 {
+	const float measurement = _ev_sample_delayed.pos(2);
+	const float measurement_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
+
+	const float bias = _ev_hgt_b_est.getBias();
+	const float bias_var = _ev_hgt_b_est.getBiasVar();
+
+	const float ev_noise = sqrtf(measurement_var);
+	_ev_hgt_b_est.setMaxStateNoise(ev_noise);
+	_ev_hgt_b_est.setProcessNoiseStdDev(ev_noise); // TODO: fix
+	_ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
+
 	// 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);
+	const float obs = measurement - bias;
+	const float obs_var = measurement_var + bias_var;
+	_ev_pos_innov(2) = _state.pos(2) - obs;
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
 
-	// observation variance - defined externally
-	float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
-
 	// _ev_pos_test_ratio(1) is the vertical test ratio
 	fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
 			     _ev_pos_innov_var(2), _ev_pos_test_ratio(1));
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 3063e0dafd..4b8e4bd61a 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -106,7 +106,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_req_hdrift(_params->req_hdrift),
 	_param_ekf2_req_vdrift(_params->req_vdrift),
 	_param_ekf2_aid_mask(_params->fusion_mode),
-	_param_ekf2_hgt_mode(_params->vdist_sensor_type),
+	_param_ekf2_hgt_mode(_params->height_sensor_ref),
 	_param_ekf2_terr_mask(_params->terrain_fusion_mode),
 	_param_ekf2_noaid_tout(_params->valid_timeout_max),
 	_param_ekf2_rng_noise(_params->range_noise),
@@ -292,7 +292,7 @@ void EKF2::Run()
 		}
 
 		// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
-		if (_params->vdist_sensor_type == 0) {
+		if (_params->fusion_mode & SensorFusionMask::USE_BARO_HGT) {
 			float sens_baro_rate = 0.f;
 
 			if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 8a4fddcc8b..2bc7fdc9cf 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -608,22 +608,31 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * 5 : Set to true to enable multi-rotor drag specific force fusion
  * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
  * 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
+ * 8 : Set to true to enable vision velocity fusion
+ * 9 : Set to true to enable barometer height fusion.
+ * 10 : Set to true to enable GPS height fusion.
+ * 11 : Set to true to enable range finder height fusion.
+ * 12 : Set to true to enable vision height fusion.
  *
  * @group EKF2
  * @min 0
- * @max 511
- * @bit 0 use GPS
- * @bit 1 use optical flow
+ * @max 8091
+ * @bit 0 position: GPS
+ * @bit 1 velocity: optical flow
  * @bit 2 inhibit IMU bias estimation
- * @bit 3 vision position fusion
- * @bit 4 vision yaw fusion
- * @bit 5 multi-rotor drag fusion
+ * @bit 3 position: vision
+ * @bit 4 yaw: vision
+ * @bit 5 wind: multi-rotor drag fusion
  * @bit 6 rotate external vision
- * @bit 7 GPS yaw fusion
- * @bit 8 vision velocity fusion
+ * @bit 7 yaw: GPS
+ * @bit 8 velocity: vision
+ * @bit 9 height: barometer
+ * @bit 10 height: GPS
+ * @bit 11 height: range finder
+ * @bit 12 height: vision
  * @reboot_required true
  */
-PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
+PARAM_DEFINE_INT32(EKF2_AID_MASK, 513);
 
 /**
  * Determines the primary source of height data used by the EKF.
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
index b077c7a5db..d4f32aacb1 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
@@ -10,9 +10,19 @@ EkfWrapper::~EkfWrapper()
 {
 }
 
-void EkfWrapper::setBaroHeight()
+void EkfWrapper::setBaroHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
+}
+
+void EkfWrapper::enableBaroHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
+}
+
+void EkfWrapper::disableBaroHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
 }
 
 bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -20,9 +30,19 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
 	return _ekf->control_status_flags().baro_hgt;
 }
 
-void EkfWrapper::setGpsHeight()
+void EkfWrapper::setGpsHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
+}
+
+void EkfWrapper::enableGpsHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
+}
+
+void EkfWrapper::disableGpsHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
 }
 
 bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -30,9 +50,19 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
 	return _ekf->control_status_flags().gps_hgt;
 }
 
-void EkfWrapper::setRangeHeight()
+void EkfWrapper::setRangeHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
+}
+
+void EkfWrapper::enableRangeHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
+}
+
+void EkfWrapper::disableRangeHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
 }
 
 bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -40,9 +70,19 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
 	return _ekf->control_status_flags().rng_hgt;
 }
 
-void EkfWrapper::setVisionHeight()
+void EkfWrapper::setVisionHeightRef()
 {
-	_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
+	_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
+}
+
+void EkfWrapper::enableVisionHeightFusion()
+{
+	_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
+}
+
+void EkfWrapper::disableVisionHeightFusion()
+{
+	_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
 }
 
 bool EkfWrapper::isIntendingVisionHeightFusion() const
diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
index 80b4cc0644..633fd0a0ce 100644
--- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
@@ -49,16 +49,24 @@ public:
 	~EkfWrapper();
 
 
-	void setBaroHeight();
+	void setBaroHeightRef();
+	void enableBaroHeightFusion();
+	void disableBaroHeightFusion();
 	bool isIntendingBaroHeightFusion() const;
 
-	void setGpsHeight();
+	void setGpsHeightRef();
+	void enableGpsHeightFusion();
+	void disableGpsHeightFusion();
 	bool isIntendingGpsHeightFusion() const;
 
-	void setRangeHeight();
+	void setRangeHeightRef();
+	void enableRangeHeightFusion();
+	void disableRangeHeightFusion();
 	bool isIntendingRangeHeightFusion() const;
 
-	void setVisionHeight();
+	void setVisionHeightRef();
+	void enableVisionHeightFusion();
+	void disableVisionHeightFusion();
 	bool isIntendingVisionHeightFusion() const;
 
 	void enableGpsFusion();
diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
index c9b503331d..c0327abe92 100644
--- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp
@@ -358,6 +358,7 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
 	// GIVEN: EKF that receives baro and GPS data
 	_sensor_simulator.startGps();
 	_sensor_simulator.runSeconds(11);
+	_ekf_wrapper.enableGpsHeightFusion();
 
 	// THEN: EKF should intend to fuse baro by default
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -396,26 +397,28 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout)
 
 	// BUT WHEN: GPS height data is also available
 	_sensor_simulator.startGps();
+	_ekf_wrapper.enableGpsHeightFusion();
 	_sensor_simulator.runSeconds(11);
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
 
 	// AND: the baro data jumps by a lot
 	_sensor_simulator._baro.setData(800.f);
 	_sensor_simulator.runSeconds(20);
 	reset_logging_checker.capturePostResetState();
 
-	// THEN: EKF should fallback to GPS height
+	// THEN: EKF should fallback to GPS height (without reset)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
-	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3));
-	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
+	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
 }
 
 TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
 {
 	// WHEN: commanding GPS height and sending GPS data
-	_ekf_wrapper.setGpsHeight();
+	_ekf_wrapper.enableGpsHeightFusion();
 	_sensor_simulator.startGps();
 	_sensor_simulator.runSeconds(11);
 
@@ -434,7 +437,7 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
 TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
 {
 	// WHEN: commanding range height and sending range data
-	_ekf_wrapper.setRangeHeight();
+	_ekf_wrapper.enableRangeHeightFusion();
 	_sensor_simulator.startRangeFinder();
 	_sensor_simulator.runSeconds(2.5f);
 	// THEN: EKF should intend to fuse range height
@@ -462,7 +465,7 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
 TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
 {
 	// WHEN: commanding vision height and sending vision data
-	_ekf_wrapper.setVisionHeight();
+	_ekf_wrapper.enableVisionHeightFusion();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(2);
 
diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp
index 436b52b3da..28befd646d 100644
--- a/src/modules/ekf2/test/test_EKF_gps.cpp
+++ b/src/modules/ekf2/test/test_EKF_gps.cpp
@@ -157,12 +157,12 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	_ekf_wrapper.enableFlowFusion();
 	_sensor_simulator.startFlow();
 
-	_ekf_wrapper.setGpsHeight();
+	_ekf_wrapper.enableGpsHeightFusion();
 
 	_sensor_simulator.runSeconds(1);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
-	EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
 
 	// WHEN: stopping GPS fusion
 	_sensor_simulator.stopGps();
diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
index 7fc0518852..4220d6559f 100644
--- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
+++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
@@ -80,7 +80,6 @@ public:
 		_ekf->set_vehicle_at_rest(true);
 
 		_ekf_wrapper.enableGpsFusion();
-		_ekf_wrapper.setBaroHeight();
 		_sensor_simulator.runSeconds(2); // Run to pass the GPS checks
 		_sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
 		EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());