AP_NavEKF2: constrain field by table after fusion

this should make for faster convergence

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -727,6 +727,11 @@ void NavEKF2_core::FuseMagnetometer()
             statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
         }
 
+        // add table constraint here for faster convergence
+        if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
+            MagTableConstrain();
+        }
+
         // the first 3 states represent the angular misalignment vector. This is
         // is used to correct the estimated quaternion on the current time step
         stateStruct.quat.rotate(stateStruct.angErr);

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -1442,6 +1442,22 @@ void NavEKF2_core::ConstrainVariances()
     for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity
 }
 
+// constrain states using WMM tables and specified limit
+void NavEKF2_core::MagTableConstrain(void)
+{
+    // constrain to error from table earth field
+    float limit_ga = frontend->_mag_ef_limit * 0.001f;
+    stateStruct.earth_magfield.x = constrain_float(stateStruct.earth_magfield.x,
+                                                   table_earth_field_ga.x-limit_ga,
+                                                   table_earth_field_ga.x+limit_ga);
+    stateStruct.earth_magfield.y = constrain_float(stateStruct.earth_magfield.y,
+                                                   table_earth_field_ga.y-limit_ga,
+                                                   table_earth_field_ga.y+limit_ga);
+    stateStruct.earth_magfield.z = constrain_float(stateStruct.earth_magfield.z,
+                                                   table_earth_field_ga.z-limit_ga,
+                                                   table_earth_field_ga.z+limit_ga);
+}
+
 // constrain states to prevent ill-conditioning
 void NavEKF2_core::ConstrainStates()
 {
@@ -1465,17 +1481,8 @@ void NavEKF2_core::ConstrainStates()
         // constrain to +/-1Ga
         for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
     } else {
-        // constrain to error from table earth field
+        // use table constrain
-        float limit_ga = frontend->_mag_ef_limit * 0.001f;
+        MagTableConstrain();
-        stateStruct.earth_magfield.x = constrain_float(stateStruct.earth_magfield.x,
-                                                       table_earth_field_ga.x-limit_ga,
-                                                       table_earth_field_ga.x+limit_ga);
-        stateStruct.earth_magfield.y = constrain_float(stateStruct.earth_magfield.y,
-                                                       table_earth_field_ga.y-limit_ga,
-                                                       table_earth_field_ga.y+limit_ga);
-        stateStruct.earth_magfield.z = constrain_float(stateStruct.earth_magfield.z,
-                                                       table_earth_field_ga.z-limit_ga,
-                                                       table_earth_field_ga.z+limit_ga);
     }
 
     // body magnetic field limit
@@ -1546,7 +1553,11 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch)
         // don't do this if the earth field has already been learned
         if (!magFieldLearned) {
             initQuat.rotation_matrix(Tbn);
+            if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
+                stateStruct.earth_magfield = table_earth_field_ga;
+            } else {
                 stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
+            }
 
             // set the NE earth magnetic field states using the published declination
             // and set the corresponding variances and covariances

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -494,6 +494,9 @@ private:
     // constrain states
     void ConstrainStates();
 
+    // constrain earth field using WMM tables
+    void MagTableConstrain(void);
+
     // fuse selected position, velocity and height measurements
     void FuseVelPosNED();