AP_NavEKF: Add pre-arm check for horizontal inertial errors

This check will declare the EKF as unhealthy if the horizontal position innovations exceed a threshold  before motors are armed.
This will help to prevent a takeoff with bad inertial data caused by bad accel or gyro offsets.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -442,8 +442,9 @@ bool NavEKF::healthy(void) const
     if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
         return false;
     }
-    // barometer innovations must be within limits when on-ground
-    if (!vehicleArmed && (fabsf(innovVelPos[5]) > 1.0f)) {
+    // barometer and position innovations must be within limits when on-ground
+    float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
+    if (!vehicleArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
         return false;
     }