AP_NavEKF2: Improve non-GPS in-flight attitude accuracy

The non-GPS mode was not being activated for small height gains - eg indoor flight.
The incorrect innovation consistency check was being applied to the synthetic velocity observations.

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED()
         // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
         // otherwise we scale it using manoeuvre acceleration
         // Use different errors if flying without GPS using synthetic position and velocity data
-        if (PV_AidingMode == AID_NONE && inFlight) {
+        if (PV_AidingMode == AID_NONE && motorsArmed) {
             // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
             // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
             R_OBS[0] = sq(10.0f);
@@ -257,6 +257,8 @@ void NavEKF2_core::FuseVelPosNED()
             // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
             R_OBS[3] = sq(25.0f);
             R_OBS[4] = R_OBS[3];
+            R_OBS[5] = posDownObsNoise;
+            for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
         } else {
             if (gpsSpdAccuracy > 0.0f) {
                 // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
@@ -270,14 +272,14 @@ void NavEKF2_core::FuseVelPosNED()
             R_OBS[1] = R_OBS[0];
             R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
             R_OBS[4] = R_OBS[3];
+            R_OBS[5] = posDownObsNoise;
+            // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
+            // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
+            // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
+            for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
+            for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
         }
-        R_OBS[5] = posDownObsNoise;
 
-        // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
-        // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
-        // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
-        for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
-        for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
 
         // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
         // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting