EKF: correct GPS data for antenna position offset

EKF/ekf.cpp

@@ -257,6 +257,20 @@ bool Ekf::update()
 				_fuse_pos = true;
 				_fuse_vert_vel = true;
 				_fuse_hor_vel = true;
+
+				// correct velocity for offset relative to IMU
+				Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
+				Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
+				Vector3f vel_offset_body = ang_rate % pos_offset_body;
+				Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body;
+				_gps_sample_delayed.vel -= vel_offset_earth;
+
+				// correct position and height for offset relative to IMU
+				Vector3f pos_offset_earth = _R_prev.transpose() * pos_offset_body;
+				_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
+				_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
+				_gps_sample_delayed.hgt += pos_offset_earth(2);
+
 			}
 
 			// only use gps height observation in the main filter if specifically enabled