AP_NavEKF2: Improve handling of GPS loss and recovery for planes

Extended GPS loss can result in the earth field states becoming  rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -151,7 +151,7 @@ void NavEKF2_core::SelectMagFusion()
         } else {
             // if we are not doing aiding with earth relative observations (eg GPS) then the declination is
             // maintained by fusing declination as a synthesised observation
-            if (PV_AidingMode != AID_ABSOLUTE) {
+            if (PV_AidingMode != AID_ABSOLUTE || (imuSampleTime_ms - lastPosPassTime_ms) > 4000) {
                 FuseDeclination();
             }
             // fuse the three magnetometer componenents sequentially

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -305,7 +305,7 @@ void NavEKF2_core::FuseVelPosNED()
                 if (PV_AidingMode == AID_ABSOLUTE) {
                     lastPosPassTime_ms = imuSampleTime_ms;
                     // if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
-                    if (posTimeout || ((varInnovVelPos[3] + varInnovVelPos[4]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
+                    if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
                         gpsPosGlitchOffsetNE.x += innovVelPos[3];
                         gpsPosGlitchOffsetNE.y += innovVelPos[4];
                         // limit the radius of the offset and decay the offset to zero radially
@@ -316,6 +316,11 @@ void NavEKF2_core::FuseVelPosNED()
                         ResetVelocity();
                         // don't fuse data on this time step
                         fusePosData = false;
+                        // Reset the position variances and corresponding covariances to a value that will pass the checks
+                        zeroRows(P,6,7);
+                        zeroCols(P,6,7);
+                        P[6][6] = sq(float(0.5f*frontend._gpsGlitchRadiusMax));
+                        P[7][7] = P[6][6];
                         // Reset the normalised innovation to avoid false failing the bad position fusion test
                         posTestRatio = 0.0f;
                         velTestRatio = 0.0f;