From 48b105b748080762e2a83901e1e73118c05581a6 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 9 Apr 2016 09:05:52 -0700
Subject: [PATCH] EKF: correct range finder data for sensor position offset

---
 EKF/ekf.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 7cafed2bc5..5378009c6e 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -218,6 +218,11 @@ bool Ekf::update()
 		// not tilted too much to use it
 		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
 		    && (_R_prev(2, 2) > 0.7071f)) {
+			// 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_prev.transpose() * pos_offset_body;
+			_range_sample_delayed.rng += pos_offset_earth(2) / _R_prev(2, 2);
+
 			// if we have range data we always try to estimate terrain height
 			_fuse_hagl_data = true;