AP_NavEKF2: fixed use of antenna position when switching GPS primary

when GPS primary switches we were using a position which had not been
corrected for antenna offset. This was used for calculating the reset
for sensor change.

This fixes that (trivial fix) and also fixes a similar issue on
position reset

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -29,6 +29,9 @@ void NavEKF2_core::ResetVelocity(void)
     zeroRows(P,3,4);
     zeroCols(P,3,4);
 
+    gps_elements gps_corrected = gpsDataNew;
+    CorrectGPSForAntennaOffset(gps_corrected);
+    
     if (PV_AidingMode != AID_ABSOLUTE) {
         stateStruct.velocity.zero();
         // set the variances using the measurement noise parameter
@@ -36,8 +39,8 @@ void NavEKF2_core::ResetVelocity(void)
     } else {
         // reset horizontal velocity states to the GPS velocity if available
         if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
-            stateStruct.velocity.x  = gpsDataNew.vel.x;
-            stateStruct.velocity.y  = gpsDataNew.vel.y;
+            stateStruct.velocity.x  = gps_corrected.vel.x;
+            stateStruct.velocity.y  = gps_corrected.vel.y;
             // set the variances using the reported GPS speed accuracy
             P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
             // clear the timeout flags and counters
@@ -83,6 +86,9 @@ void NavEKF2_core::ResetPosition(void)
     zeroRows(P,6,7);
     zeroCols(P,6,7);
 
+    gps_elements gps_corrected = gpsDataNew;
+    CorrectGPSForAntennaOffset(gps_corrected);
+    
     if (PV_AidingMode != AID_ABSOLUTE) {
         // reset all position state history to the last known position
         stateStruct.position.x = lastKnownPositionNE.x;
@@ -93,10 +99,10 @@ void NavEKF2_core::ResetPosition(void)
         // Use GPS data as first preference if fresh data is available
         if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
             // record the ID of the GPS for the data we are using for the reset
-            last_gps_idx = gpsDataNew.sensor_idx;
+            last_gps_idx = gps_corrected.sensor_idx;
             // write to state vector and compensate for offset  between last GPS measurement and the EKF time horizon
-            stateStruct.position.x = gpsDataNew.pos.x  + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
-            stateStruct.position.y = gpsDataNew.pos.y  + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
+            stateStruct.position.x = gps_corrected.pos.x  + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
+            stateStruct.position.y = gps_corrected.pos.y  + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
             // set the variances using the position measurement noise parameter
             P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
             // clear the timeout flags and counters
@@ -242,6 +248,33 @@ bool NavEKF2_core::resetHeightDatum(void)
     return true;
 }
 
+/*
+  correct GPS data for position offset of antenna phase centre relative to the IMU
+*/
+void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data)
+{
+    const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
+    if (posOffsetBody.is_zero()) {
+        return;
+    }
+
+    // Don't fuse velocity data if GPS doesn't support it
+    if (fuseVelData) {
+        // TODO use a filtered angular rate with a group delay that matches the GPS delay
+        Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
+        Vector3f velOffsetBody = angRate % posOffsetBody;
+        Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
+        gps_data.vel.x -= velOffsetEarth.x;
+        gps_data.vel.y -= velOffsetEarth.y;
+        gps_data.vel.z -= velOffsetEarth.z;
+    }
+
+    Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
+    gps_data.pos.x -= posOffsetEarth.x;
+    gps_data.pos.y -= posOffsetEarth.y;
+    gps_data.hgt += posOffsetEarth.z;
+}
+
 /********************************************************
 *                   FUSE MEASURED_DATA                  *
 ********************************************************/
@@ -275,25 +308,8 @@ void NavEKF2_core::SelectVelPosFusion()
         fusePosData = true;
         extNavUsedForPos = false;
 
-        // correct GPS data for position offset of antenna phase centre relative to the IMU
-        Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
-        if (!posOffsetBody.is_zero()) {
-            // Don't fuse velocity data if GPS doesn't support it
-            if (fuseVelData) {
-                // TODO use a filtered angular rate with a group delay that matches the GPS delay
-                Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
-                Vector3f velOffsetBody = angRate % posOffsetBody;
-                Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
-                gpsDataDelayed.vel.x -= velOffsetEarth.x;
-                gpsDataDelayed.vel.y -= velOffsetEarth.y;
-                gpsDataDelayed.vel.z -= velOffsetEarth.z;
-            }
-
-            Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-            gpsDataDelayed.pos.x -= posOffsetEarth.x;
-            gpsDataDelayed.pos.y -= posOffsetEarth.y;
-            gpsDataDelayed.hgt += posOffsetEarth.z;
-        }
+        // correct for antenna position
+        CorrectGPSForAntennaOffset(gpsDataDelayed);
 
         // copy corrected GPS data to observation vector
         if (fuseVelData) {
@@ -355,8 +371,8 @@ void NavEKF2_core::SelectVelPosFusion()
         posResetNE.y = stateStruct.position.y;
 
         // Set the position states to the position from the new GPS
-        stateStruct.position.x = gpsDataNew.pos.x;
-        stateStruct.position.y = gpsDataNew.pos.y;
+        stateStruct.position.x = gpsDataDelayed.pos.x;
+        stateStruct.position.y = gpsDataDelayed.pos.y;
 
         // Calculate the position offset due to the reset
         posResetNE.x = stateStruct.position.x - posResetNE.x;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -776,6 +776,9 @@ private:
     // update timing statistics structure
     void updateTimingStatistics(void);
 
+    // correct gps data for antenna position
+    void CorrectGPSForAntennaOffset(gps_elements &gps_data);
+
     // Length of FIFO buffers used for non-IMU sensor data.
     // Must be larger than the time period defined by IMU_BUFFER_LENGTH
     static const uint32_t OBS_BUFFER_LENGTH = 5;