ekf_helper: fixed resetting height using range finder

- take range sensor offset wrt to IMU into account
- use projection to earth Z axis

Signed-off-by: Roman <bapstroman@gmail.com>

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;