EKF: protect declination fusion from badly conditioned earth field es…

…timates

Fusion with large initial magnetometer biases errors can result in the the NE earth field states reducing in magnitude and effectively flipping sign.

EKF: Move declination state limiting into a separate function

EKF: Limit NE mag states after each 3-axis mag fusion

EKF: Fix bug in mag field strength look-up scale factor

EKF/ekf.h

@@ -494,6 +494,9 @@ class Ekf : public EstimatorInterface
 	// argument passed in is the declination uncertainty in radians
 	void fuseDeclination(float decl_sigma);
 
+	// apply sensible limits to the declination and length of the NE mag field states estimates
+	void limitDeclination();
+
 	// fuse airspeed measurement
 	void fuseAirspeed();
 

EKF/estimator_interface.h

@@ -552,6 +552,8 @@ class EstimatorInterface
 
 	float _mag_declination_gps{0.0f};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
 	float _mag_declination_to_save_deg{0.0f}; // magnetic declination to save to EKF2_MAG_DECL (deg)
+	float _mag_inclination_gps{0.0f};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _mag_strength_gps{0.0f};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
 
 	// this is the current status of the filter control modes
 	filter_control_status_u _control_status{};

EKF/gps_checks.cpp

@@ -77,8 +77,12 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 		_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
 		_NED_origin_initialised = true;
 		_last_gps_origin_time_us = _time_last_imu;
-		// set the magnetic declination returned by the geo library using the current GPS position
+
+		// set the magnetic field data returned by the geo library using the current GPS position
 		_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
+		_mag_inclination_gps = math::radians(get_mag_inclination(lat, lon));
+		_mag_strength_gps = 0.01f * get_mag_strength(lat, lon);
+
 		// request a reset of the yaw using the new declination
 		_mag_yaw_reset_req = true;
 		// save the horizontal and vertical position uncertainty of the origin

EKF/mag_fusion.cpp

@@ -419,6 +419,8 @@ void Ekf::fuseMag()
 			// apply the state corrections
 			fuse(Kfusion, _mag_innov[index]);
 
+			// constrain the declination of the earth field states
+			limitDeclination();
 		}
 	}
 }
@@ -797,14 +799,18 @@ void Ekf::fuseDeclination(float decl_sigma)
 	float magN = _state.mag_I(0);
 	float magE = _state.mag_I(1);
 
+	// minimum horizontal field strength before calculation becomes badly conditioned (T)
+	float h_field_min = 0.001f;
+
+	// observation variance (rad**2)
 	float R_DECL = sq(decl_sigma);
 
 	// Calculate intermediate variables
 	float t2 = magE*magE;
 	float t3 = magN*magN;
 	float t4 = t2+t3;
 	// if the horizontal magnetic field is too small, this calculation will be badly conditioned
-	if (t4 < 1e-4f) {
+	if (t4 < h_field_min*h_field_min) {
 		return;
 	}
 	float t5 = P[16][16]*t2;
@@ -922,5 +928,57 @@ void Ekf::fuseDeclination(float decl_sigma)
 		// apply the state corrections
 		fuse(Kfusion, innovation);
 
+		// constrain the declination of the earth field states
+		limitDeclination();
+	}
+}
+
+void Ekf::limitDeclination()
+{
+	// get a reference value for the earth field declinaton and minimum plausible horizontal field strength
+	// set to 50% of the horizontal strength from geo tables if location is known
+	float decl_reference;
+	float h_field_min = 0.001f;
+	if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
+		// use parameter value until GPS is available, then use value returned by geo library
+		if (_NED_origin_initialised) {
+			decl_reference = _mag_declination_gps;
+			h_field_min = fmaxf(h_field_min , 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps));
+		} else {
+			decl_reference = math::radians(_params.mag_declination_deg);
+		}
+	} else {
+		// always use the parameter value
+		decl_reference = math::radians(_params.mag_declination_deg);
+	}
+
+	// do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned
+	// and can result in a reversal of the NE field states which the filter cannot recover from
+	// apply a circular limit
+	float h_field = sqrtf(_state.mag_I(0)*_state.mag_I(0) + _state.mag_I(1)*_state.mag_I(1));
+	if (h_field < h_field_min) {
+		if (h_field > 0.001f * h_field_min) {
+			float h_scaler = h_field_min / h_field;
+			_state.mag_I(0) *= h_scaler;
+			_state.mag_I(1) *= h_scaler;
+		} else {
+			// too small to scale radially so set to expected value
+			_state.mag_I(0) = 2.0f * h_field_min * cosf(_mag_declination);
+			_state.mag_I(1) = 2.0f * h_field_min * sinf(_mag_declination);
+		}
+		h_field = h_field_min;
+	}
+
+	// do not allow the declination estimate to vary too much relative to the reference value
+	const float decl_tolerance = 0.5f;
+	const float decl_max = decl_reference + decl_tolerance;
+	const float decl_min = decl_reference - decl_tolerance;
+	const float decl_estimate = atan2(_state.mag_I(1) , _state.mag_I(0));
+	if (decl_estimate > decl_max)  {
+		_state.mag_I(0) = h_field * cosf(decl_max);
+		_state.mag_I(1) = h_field * sinf(decl_max);
+	} else if (decl_estimate < decl_min)  {
+		_state.mag_I(0) = h_field * cosf(decl_min);
+		_state.mag_I(1) = h_field * sinf(decl_min);
 	}
 }

geo_lookup/geo_mag_declination.cpp

@@ -93,7 +93,7 @@ static constexpr const int8_t inclination_table[13][37] = \
 	{ 71,71,72,73,75,77,78,80,81,81,80,79,77,76,74,73,73,73,73,73,73,74,74,75,76,77,78,78,78,78,77,75,73,72,71,71,71 },
 };
 
-// strength data in mTesla
+// strength data in centi-Tesla
 static constexpr const int8_t strength_table[13][37] = \
 {
 	{ 62,60,58,56,54,52,49,46,43,41,38,36,34,32,31,31,30,30,30,31,33,35,38,42,46,51,55,59,62,64,66,67,67,66,65,64,62 },

geo_lookup/geo_mag_declination.h

@@ -47,5 +47,5 @@ float get_mag_declination(float lat, float lon);
 // Return magnetic field inclination in degrees
 float get_mag_inclination(float lat, float lon);
 
-// return magnetic field strength in mTesla
+// return magnetic field strength in centi-Tesla
 float get_mag_strength(float lat, float lon);