diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp
index 1dde77dafe..ba459a081b 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp
@@ -3968,29 +3968,33 @@ void NavEKF_core::readGpsData()
             // Monitor qulaity of GPS data inflight
             calcGpsGoodForFlight();
 
-            // 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
+            // Read the GPS locaton in WGS-84 lat,long,height coordinates
             const struct Location &gpsloc = _ahrs->get_gps().location();
+
+            // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
+            if (!validOrigin && gpsGoodToAlign) {
+                // Set the NE origin to the current GPS position if not previously set
+                if (!validOrigin) {
+                    setOrigin();
+                    // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
+                    alignMagStateDeclination();
+                    // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
+                    EKF_origin.alt = gpsloc.alt - hgtMea;
+                }
+            }
+
+            // Commence GPS aiding when able to
+            if ((frontend._fusionModeGPS != 3) && (PV_AidingMode != AID_ABSOLUTE) && vehicleArmed && gpsGoodToAlign) {
+                PV_AidingMode = AID_ABSOLUTE;
+                constPosMode = false;
+                // Initialise EKF position and velocity states to last GPS measurement
+                ResetPosition();
+                ResetVelocity();
+            }
+
+            // Convert to local coordinates if we have an origin.
             if (validOrigin) {
                 gpsPosNE = location_diff(EKF_origin, gpsloc);
-            } else if (gpsGoodToAlign){
-                // Set the NE origin to the current GPS position
-                setOrigin();
-                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
-                alignMagStateDeclination();
-                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
-                EKF_origin.alt = gpsloc.alt - hgtMea;
-                // We are by definition at the origin at the instant of alignment so set NE position to zero
-                gpsPosNE.zero();
-                // If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode
-                if (vehicleArmed && frontend._fusionModeGPS != 3) {
-                    constPosMode = false;
-                    PV_AidingMode = AID_ABSOLUTE;
-                    gpsNotAvailable = false;
-                    // Initialise EKF position and velocity states
-                    ResetPosition();
-                    ResetVelocity();
-                }
             }
         } else {
              // report GPS fix status