AP_NavEKF2: improve switch-over to use of mag field states

Set initial variances to measurement uncertainties
Always perform a field  and yaw reset

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -67,7 +67,18 @@ void NavEKF2_core::setWindMagStateLearningMode()
     bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
 
     // Inhibit the magnetic field calibration if not requested or denied
-    inhibitMagStates = (!magCalRequested || magCalDenied);
+    bool setMagInhibit = !magCalRequested || magCalDenied;
+    if (!inhibitMagStates && setMagInhibit) {
+        inhibitMagStates = true;
+    } else if (inhibitMagStates && !setMagInhibit) {
+        inhibitMagStates = false;
+        // when commencing use of magnetic field states, set the variances equal to the observation uncertainty
+        for (uint8_t index=16; index<=21; index++) {
+            P[index][index] = sq(frontend->_magNoise);
+        }
+        // let the magnetometer fusion know it needs to reset the yaw and field states
+        firstMagYawInit = false;
+    }
 
     // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
     // because we want it re-done for each takeoff