From e884e9cc6e437e7174df654ff74d861277f7e00e Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 12 Oct 2016 09:02:27 +1100
Subject: [PATCH] AP_NavEKF2: Correct GPS data for antenna body frame offset

---
 .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp     | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 51bf735b19..7816683beb 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -192,10 +192,28 @@ void NavEKF2_core::SelectVelPosFusion()
         // Don't fuse velocity data if GPS doesn't support it
         if (frontend->_fusionModeGPS <= 1) {
             fuseVelData = true;
+
         } else {
             fuseVelData = false;
         }
         fusePosData = true;
+
+        // correct GPS velocity data for position offset relative to the IMU
+        // TOTO use a filtered angular rate with a group delay that matches the GPS delay
+        Vector3f posOffsetBody = gpsDataDelayed.body_offset - accelPosOffset;
+        if (fuseVelData) {
+            Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
+            Vector3f velOffsetBody = angRate % posOffsetBody;
+            Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
+            gpsDataDelayed.vel -= velOffsetEarth;
+        }
+
+        // correct GPS position data for position offset relative to the IMU
+        Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+        gpsDataDelayed.pos.x -= posOffsetEarth.x;
+        gpsDataDelayed.pos.y -= posOffsetEarth.y;
+        gpsDataDelayed.hgt += posOffsetEarth.z;
+
     } else {
         fuseVelData = false;
         fusePosData = false;