AP_NavEKF: Make NED origin independent of home position

This enables the filter to report the last known position after disarm.
The LLH location of the filters NED origin is published and should be logged every time the vehicle is armed to assist with post-flight trajectory reconstruction.
The LLH location of the filters NED origin can be set externally whilst the vehicle is disarmed.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -3639,17 +3639,21 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
 // return the last calculated latitude, longitude and height
 bool NavEKF::getLLH(struct Location &loc) const
 {
-    loc.lat = _ahrs->get_home().lat;
-    loc.lng = _ahrs->get_home().lng;
-    loc.alt = _ahrs->get_home().alt - state.position.z*100;
-    loc.flags.relative_alt = 0;
-    loc.flags.terrain_alt = 0;
-    if (constPosMode) {
-        location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
+    if(validOrigin) {
+        loc.lat = EKF_origin.lat;
+        loc.lng = EKF_origin.lng;
+        loc.alt = EKF_origin.alt - state.position.z*100;
+        loc.flags.relative_alt = 0;
+        loc.flags.terrain_alt = 0;
+        if (constPosMode) {
+            location_offset(loc, lastKnownPositionNE.x, lastKnownPositionNE.y);
+        } else {
+            location_offset(loc, state.position.x, state.position.y);
+        }
+        return true;
     } else {
-        location_offset(loc, state.position.x, state.position.y);
+        return false;
     }
-    return true;
 }
 
 // return the estimated height above ground level
@@ -3964,9 +3968,15 @@ void NavEKF::readGpsData()
             }
         }
 
-        // read latitutde and longitude from GPS and convert to NE position
-        const struct Location &gpsloc = _ahrs->get_gps().location();
-        gpsPosNE = location_diff(_ahrs->get_home(), gpsloc);
+        // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
+        // If we don't have an origin, then set it to the current GPS coordinates
+        if (validOrigin) {
+            const struct Location &gpsloc = _ahrs->get_gps().location();
+            gpsPosNE = location_diff(EKF_origin, gpsloc);
+        } else {
+            setOrigin();
+            gpsPosNE.zero();
+        }
         // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
         decayGpsOffset();
     }
@@ -4372,6 +4382,7 @@ void NavEKF::InitialiseVariables()
     inhibitMagStates = true;
     gndOffsetValid =  false;
     flowXfailed = false;
+    validOrigin = false;
 }
 
 // return true if we should use the airspeed sensor
@@ -4548,8 +4559,11 @@ void NavEKF::performArmingChecks()
         }
         // zero stored velocities used to do dead-reckoning
         heldVelNE.zero();
-        // zero last known position used to deadreckon if GPS is lost
-        lastKnownPositionNE.zero();
+        // zero last known position used to deadreckon if GPS is lost when arming
+        // keep position during disarm to provide continuing indication of last known position
+        if (vehicleArmed) {
+            lastKnownPositionNE.zero();
+        }
         // set various  useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
         if (!vehicleArmed) {
             PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
@@ -4598,4 +4612,31 @@ void NavEKF::performArmingChecks()
 
 }
 
+// Set the NED origin to be used until the next filter reset
+void NavEKF::setOrigin()
+{
+    EKF_origin = _ahrs->get_gps().location();
+    validOrigin = true;
+}
+
+// return the LLH location of the filters NED origin
+bool NavEKF::getOriginLLH(struct Location &loc) const
+{
+    if (validOrigin) {
+        loc = EKF_origin;
+    }
+    return validOrigin;
+}
+
+// set the LLH location of the filters NED origin
+bool NavEKF::setOriginLLH(struct Location &loc)
+{
+    if (vehicleArmed) {
+        return false;
+    }
+    EKF_origin = loc;
+    validOrigin = true;
+    return true;
+}
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF/AP_NavEKF.h

@@ -139,6 +139,17 @@ public:
     // return the last calculated latitude, longitude and height
     bool getLLH(struct Location &loc) const;
 
+    // return the latitude and longitude and height used to set the NED origin
+    // All NED positions calculated by the filter are relative to this location
+    // Returns false if the origin has not been set
+    bool getOriginLLH(struct Location &loc) const;
+
+    // set the latitude and longitude and height used to set the NED origin
+    // All NED positions calcualted by the filter will be relative to this location
+    // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
+    // Returns false if the filter has rejected the attempt to set the origin
+    bool setOriginLLH(struct Location &loc);
+
     // return estimated height above ground level
     // return false if ground height is not being estimated.
     bool getHAGL(float &HAGL) const;
@@ -379,6 +390,9 @@ private:
     // Check arm status and perform required checks and mode changes
     void performArmingChecks();
 
+    // Set the NED origin to be used until the next filter reset
+    void setOrigin();
+
     // EKF Mavlink Tuneable Parameters
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s
@@ -572,6 +586,8 @@ private:
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
     bool vehicleArmed;              // true when the vehicle is disarmed
     bool prevVehicleArmed;          // vehicleArmed from previous frame
+    struct Location EKF_origin;     // LLH origin of the NED axis system - do not change unless filter is reset
+    bool validOrigin;               // true when the EKF origin is valid
 
     // Used by smoothing of state corrections
     float gpsIncrStateDelta[10];    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement