AP_NavEKF: Allow 10 seconds for the filter to settle after initialisation

Filter is declared unhealthy and will not arm for first 10 seconds after initialisation

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -441,6 +441,10 @@ bool NavEKF::healthy(void) const
         // extremely unhealthy.
         return false;
     }
+    // Give the filter 10 seconds to settle before use
+    if ((imuSampleTime_ms - ekfStartTime_ms) < 10000) {
+        return false;
+    }
 
     // all OK
     return true;
@@ -684,9 +688,9 @@ void NavEKF::UpdateFilter()
     // check if on ground
     SetFlightAndFusionModes();
 
-    // determine vehicle arm status
+    // determine vehicle arm status and don't allow filter to arm until it has been running for long enough to stabilise
     prevVehicleArmed = vehicleArmed;
-    vehicleArmed = getVehicleArmStatus();
+    vehicleArmed = (getVehicleArmStatus() && (imuSampleTime_ms - ekfStartTime_ms) > 10000);
 
     // check to see if arm status has changed and reset states if it has
     if (vehicleArmed != prevVehicleArmed) {
@@ -4353,6 +4357,7 @@ void NavEKF::ZeroVariables()
     flowMeaTime_ms = imuSampleTime_ms;
     prevFlowFusionTime_ms = imuSampleTime_ms;
     rngMeaTime_ms = imuSampleTime_ms;
+    ekfStartTime_ms = imuSampleTime_ms;
 
     gpsNoiseScaler = 1.0f;
     velTimeout = true;

libraries/AP_NavEKF/AP_NavEKF.h

@@ -541,6 +541,7 @@ private:
     uint32_t timeAtLastAuxEKF_ms;   // last time the auxilliary filter was run to fuse range or optical flow measurements
     uint32_t secondLastFixTime_ms;  // time of second last GPS fix used to determine how long since last update
     uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
+    uint32_t ekfStartTime_ms;       // time the EKF was started (msec)
     Vector3f lastAngRate;           // angular rate from previous IMU sample used for trapezoidal integrator
     Vector3f lastAccel1;            // acceleration from previous IMU1 sample used for trapezoidal integrator
     Vector3f lastAccel2;            // acceleration from previous IMU2 sample used for trapezoidal integrator