AP_NavEKF3: gps_elements gets corrected flag

CorrectGPSForAntennaOffset uses this flag to ensure position and velocity are only corrected once

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -581,6 +581,9 @@ void NavEKF3_core::readGpsData()
             // read the NED velocity from the GPS
             gpsDataNew.vel = gps.velocity(selected_gps);
 
+            // position and velocity are not yet corrected for sensor position
+            gpsDataNew.corrected = false;
+
             // Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
             // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
             float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -323,6 +323,12 @@ bool NavEKF3_core::resetHeightDatum(void)
  */
 void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
 {
+    // return immediately if already corrected
+    if (gps_data.corrected) {
+        return;
+    }
+    gps_data.corrected = true;
+
     const Vector3f &posOffsetBody = AP::gps().get_antenna_offset(gps_data.sensor_idx) - accelPosOffset;
     if (posOffsetBody.is_zero()) {
         return;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -560,6 +560,7 @@ private:
         Vector3f    vel;            // velocity of the GPS antenna in local NED earth frame (m/sec)
         uint32_t    time_ms;        // measurement timestamp (msec)
         uint8_t     sensor_idx;     // unique integer identifying the GPS sensor
+        bool        corrected;      // true when the position and velocity have been corrected for sensor position
     };
 
     struct mag_elements {