AP_NavEKF3: implement moving origin

this shifts EKF_origin to the current location at 1Hz, while leaving
public_origin alone. All output APIs and logging are relative to
public_origin.

The effect of this change is to remove the distortion caused by a
spherical earth, which allows the EKF to operate without errors at
very long distances from the public_origin.

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -611,9 +611,12 @@ bool NavEKF3_core::setOrigin(const Location &loc)
     validOrigin = true;
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
 
-    // put origin in frontend as well to ensure it stays in sync between lanes
+    if (!frontend->common_origin_valid) {
-    frontend->common_EKF_origin = EKF_origin;
+        frontend->common_origin_valid = true;
-    frontend->common_origin_valid = true;
+        // put origin in frontend as well to ensure it stays in sync between lanes
+        public_origin = EKF_origin;
+    }
+
 
     return true;
 }

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -648,16 +648,6 @@ void NavEKF3_core::readGpsData()
             // Post-alignment checks
             calcGpsGoodForFlight();
 
-            // see if we can get an origin from the frontend
-            if (!validOrigin && frontend->common_origin_valid) {
-
-                if (!setOrigin(frontend->common_EKF_origin)) {
-                    // set an error as an attempt was made to set the origin more than once
-                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-                    return;
-                }
-            }
-
             // Read the GPS location in WGS-84 lat,long,height coordinates
             const struct Location &gpsloc = gps.location(selected_gps);
 
@@ -700,7 +690,8 @@ void NavEKF3_core::readGpsData()
 
             // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
             if (validOrigin) {
-                gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
+                gpsDataNew.lat = gpsloc.lat;
+                gpsDataNew.lng = gpsloc.lng;
                 if ((frontend->_originHgtMode & (1<<2)) == 0) {
                     gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
                 } else {
@@ -1463,15 +1454,14 @@ void NavEKF3_core::getEarthFieldTable(const Location &loc)
     table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7,
                                                                 loc.lng*1.0e-7));
     have_table_earth_field = true;
-    last_field_update_ms = imuSampleTime_ms;
 }
 
 /*
-  check if we should update the earth field, we update it at 1Hz
+  update earth field, called at 1Hz
  */
 void NavEKF3_core::checkUpdateEarthField(void)
 {
-    if (have_table_earth_field && imuSampleTime_ms - last_field_update_ms > 1000) {
+    if (have_table_earth_field && filterStatus.flags.using_gps) {
         Location loc = EKF_origin;
         loc.offset(stateStruct.position.x, stateStruct.position.y);
         getEarthFieldTable(loc);

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -207,8 +207,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
     if (PV_AidingMode != AID_NONE) {
         // This is the normal mode of operation where we can use the EKF position states
         // correct for the IMU offset (EKF calculations are at the IMU)
-        posNE.x = outputDataNew.position.x + posOffsetNED.x;
+        posNE = (outputDataNew.position.xy() + posOffsetNED.xy() + public_origin.get_distance_NE_ftype(EKF_origin)).tofloat();
-        posNE.y = outputDataNew.position.y + posOffsetNED.y;
         return true;
 
     } else {
@@ -218,9 +217,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
             if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
                 // If the origin has been set and we have GPS, then return the GPS position relative to the origin
                 const struct Location &gpsloc = gps.location(selected_gps);
-                const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
+                posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
-                posNE.x = tempPosNE.x;
-                posNE.y = tempPosNE.y;
                 return false;
             } else if (rngBcnAlignmentStarted) {
                 // If we are attempting alignment using range beacon data, then report the position
@@ -229,14 +226,12 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
                 return false;
             } else {
                 // If no GPS fix is available, all we can do is provide the last known position
-                posNE.x = outputDataNew.position.x;
+                posNE = outputDataNew.position.xy().tofloat();
-                posNE.y = outputDataNew.position.y;
                 return false;
             }
         } else {
             // If the origin has not been set, then we have no means of providing a relative position
-            posNE.x = 0.0f;
+            posNE.zero();
-            posNE.y = 0.0f;
             return false;
         }
     }
@@ -357,7 +352,7 @@ void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGa
 bool NavEKF3_core::getOriginLLH(struct Location &loc) const
 {
     if (validOrigin) {
-        loc = EKF_origin;
+        loc = public_origin;
         // report internally corrected reference height if enabled
         if ((frontend->_originHgtMode & (1<<2)) == 0) {
             loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -95,9 +95,13 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
             CorrectGPSForAntennaOffset(gps_corrected);
             // record the ID of the GPS for the data we are using for the reset
             last_gps_idx = gps_corrected.sensor_idx;
-            // write to state vector and compensate for offset  between last GPS measurement and the EKF time horizon
+            // calculate position
-            stateStruct.position.x = gps_corrected.pos.x  + 0.001*gps_corrected.vel.x*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms));
+            const Location gpsloc{gps_corrected.lat, gps_corrected.lng, 0, Location::AltFrame::ABSOLUTE};
-            stateStruct.position.y = gps_corrected.pos.y  + 0.001*gps_corrected.vel.y*(ftype(imuDataDelayed.time_ms) - ftype(gps_corrected.time_ms));
+            stateStruct.position.xy() = EKF_origin.get_distance_NE_ftype(gpsloc);
+            // compensate for offset  between last GPS measurement and the EKF time horizon. Note that this is an unusual
+            // time delta in that it can be both -ve and +ve
+            const int32_t tdiff = imuDataDelayed.time_ms - gps_corrected.time_ms;
+            stateStruct.position.xy() += gps_corrected.vel.xy()*0.001*tdiff;
             // set the variances using the position measurement noise parameter
             P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
         } else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
@@ -335,8 +339,7 @@ void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
     gps_data.vel -= velOffsetEarth;
 
     Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
-    gps_data.pos.x -= posOffsetEarth.x;
+    Location::offset_latlng(gps_data.lat, gps_data.lng, -posOffsetEarth.x, -posOffsetEarth.y);
-    gps_data.pos.y -= posOffsetEarth.y;
     gps_data.hgt += posOffsetEarth.z;
 }
 
@@ -349,6 +352,9 @@ void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
     }
     ext_nav_data.corrected = true;
 
+    // external nav data is against the public_origin, so convert to offset from EKF_origin
+    ext_nav_data.pos.xy() += EKF_origin.get_distance_NE_ftype(public_origin);
+
 #if HAL_VISUALODOM_ENABLED
     const auto *visual_odom = dal.visualodom();
     if (visual_odom == nullptr) {
@@ -477,8 +483,10 @@ void NavEKF3_core::SelectVelPosFusion()
             velPosObs[1] = gpsDataDelayed.vel.y;
             velPosObs[2] = gpsDataDelayed.vel.z;
         }
-        velPosObs[3] = gpsDataDelayed.pos.x;
+        const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
-        velPosObs[4] = gpsDataDelayed.pos.y;
+        const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
+        velPosObs[3] = posxy.x;
+        velPosObs[4] = posxy.y;
 #if EK3_FEATURE_EXTERNAL_NAV
     } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
         // use external nav system for horizontal position
@@ -517,7 +525,9 @@ void NavEKF3_core::SelectVelPosFusion()
         last_gps_idx = gpsDataDelayed.sensor_idx;
 
         // reset the position to the GPS position
-        ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y);
+        const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
+        const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
+        ResetPositionNE(posxy.x, posxy.y);
 
         // If we are also using GPS as the height reference, reset the height
         if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) {

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -10,7 +10,8 @@
 // constructor
 NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
     frontend(_frontend),
-    dal(AP::dal())
+    dal(AP::dal()),
+    public_origin(frontend->common_EKF_origin)
 {
     firstInitTime_ms = 0;
     lastInitFailReport_ms = 0;
@@ -701,8 +702,12 @@ void NavEKF3_core::UpdateFilter(bool predict)
         // Update the filter status
         updateFilterStatus();
 
-        // check for update of earth field
+        if (imuSampleTime_ms - last_oneHz_ms >= 1000) {
-        checkUpdateEarthField();
+            // 1Hz tasks
+            last_oneHz_ms = imuSampleTime_ms;
+            moveEKFOrigin();
+            checkUpdateEarthField();
+        }
     }
 
     // Wind output forward from the fusion to output time horizon
@@ -2146,3 +2151,31 @@ void NavEKF3_core::verifyTiltErrorVariance()
     }
 }
 #endif
+
+/*
+  move the EKF origin to the current position at 1Hz. The public_origin doesn't move.
+  By moving the EKF origin we keep the distortion due to spherical
+  shape of the earth to a minimum.
+ */
+void NavEKF3_core::moveEKFOrigin(void)
+{
+    // only move origin when we have a origin and we're using GPS
+    if (!frontend->common_origin_valid || !filterStatus.flags.using_gps) {
+        return;
+    }
+
+    // move the origin to the current state location
+    Location loc = EKF_origin;
+    loc.offset(stateStruct.position.x, stateStruct.position.y);
+    const Vector2F diffNE = loc.get_distance_NE_ftype(EKF_origin);
+    EKF_origin = loc;
+
+    // now fix all output states
+    stateStruct.position.xy() += diffNE;
+    outputDataNew.position.xy() += diffNE;
+    outputDataDelayed.position.xy() += diffNE;
+
+    for (unsigned index=0; index < imu_buffer_length; index++) {
+        storedOutput[index].position.xy() += diffNE;
+    }
+}

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -499,7 +499,7 @@ private:
     };
 
     struct gps_elements : EKF_obs_element_t {
-        Vector2F    pos;            // horizontal North East position of the GPS antenna in local NED earth frame (m)
+        int32_t     lat, lng;       // latitude and longitude in 1e7 degrees
         ftype       hgt;            // height of the GPS antenna in local NED earth frame (m)
         Vector3F    vel;            // velocity of the GPS antenna in local NED earth frame (m/sec)
         uint8_t     sensor_idx;     // unique integer identifying the GPS sensor
@@ -1025,7 +1025,8 @@ private:
     bool needEarthBodyVarReset;     // we need to reset mag earth variances at next CovariancePrediction
     bool inhibitDelAngBiasStates;   // true when IMU delta angle bias states are inactive
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
-    struct Location EKF_origin;     // LLH origin of the NED axis system
+    struct Location EKF_origin;     // LLH origin of the NED axis system, internal only
+    struct Location &public_origin; // LLH origin of the NED axis system, public functions
     bool validOrigin;               // true when the EKF origin is valid
     ftype gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the GPS receiver
     ftype gpsPosAccuracy;           // estimated position accuracy in m returned by the GPS receiver
@@ -1403,7 +1404,13 @@ private:
     bool have_table_earth_field;   // true when we have initialised table_earth_field_ga
     Vector3F table_earth_field_ga; // earth field from WMM tables
     ftype table_declination;       // declination in radians from the tables
-    uint32_t last_field_update_ms;
+
+    // 1Hz update
+    uint32_t last_oneHz_ms;
+    void oneHzUpdate(void);
+
+    // move EKF origin at 1Hz
+    void moveEKFOrigin(void);
 
     // handle earth field updates
     void getEarthFieldTable(const Location &loc);