diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 49989e30b0..5bc1747346 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -158,8 +158,12 @@ void Ekf::resetHeight()
 		rangeSample range_newest = _range_buffer.get_newest();
 
 		if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
+			// correct the range data for position offset relative to the IMU
+			Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+			Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+			range_newest.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
 			// calculate the new vertical position using range sensor
-			float new_pos_down = _hgt_sensor_offset - range_newest.rng;
+			float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
 
 			// update the state and assoicated variance
 			_state.pos(2) = new_pos_down;