diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index c550751f42..12c2fc9faa 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -4403,7 +4403,6 @@ void NavEKF::InitialiseVariables()
     dtIMUactual = 0.0025f;
     dt = 0;
     hgtMea = 0;
-    firstArmPosD = 0.0f;
     storeIndex = 0;
     lastGyroBias.zero();
 	prevDelAng.zero();
@@ -4667,7 +4666,6 @@ void NavEKF::performArmingChecks()
         if (vehicleArmed && !firstArmComplete) {
             firstArmComplete = true;
             state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
-            firstArmPosD = state.position.z;
         }
         // zero stored velocities used to do dead-reckoning
         heldVelNE.zero();
@@ -4723,12 +4721,12 @@ void NavEKF::performArmingChecks()
         ResetHeight();
         StoreStatesReset();
 
-    } else if (vehicleArmed && !firstMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
+    } else if (vehicleArmed && !firstMagYawInit && state.position.z < -1.5f && !assume_zero_sideslip()) {
         // Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
         // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
         state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
         firstMagYawInit = true;
-    } else if (vehicleArmed && !secondMagYawInit && firstArmPosD - state.position.z > 5.0f && !assume_zero_sideslip()) {
+    } else if (vehicleArmed && !secondMagYawInit && state.position.z < -5.0f && !assume_zero_sideslip()) {
         // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
         // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
         state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 944a72e5af..88dcf25531 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -611,7 +611,6 @@ private:
     float tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
     bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
-    float firstArmPosD;             // vertical position at the first arming (transition from sttatic mode) after start up
     bool firstArmComplete;          // true when first transition out of static mode has been performed after start up
     bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
     bool secondMagYawInit;          // true when the second post takeoff initialisation of earth field and yaw angle has been performed