diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 4021a728f4e4a63e45393e365b401919706ef80d..eb387f527dc7ec6de94e4ce0e56570d09b80fb86 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 3d9be0122128af9ff82e197b48b76dc1c5ea1ed8..9850d8448d0d1575022b4e205fa1238651589677 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/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