ekf2: make mag control responsible for WMM

- this further untangles mag control (which requires the WMM) from GPS

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -41,10 +41,6 @@
 
 #include "ekf.h"
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-# include <lib/world_magnetic_model/geo_mag_declination.h>
-#endif // CONFIG_EKF2_MAGNETOMETER
-
 #include <mathlib/mathlib.h>
 
 // GPS pre-flight check bit locations
@@ -89,20 +85,20 @@ void Ekf::collect_gps(const gnssSample &gps)
 		_gpos_origin_eph = gps.hacc;
 		_gpos_origin_epv = gps.vacc;
 
-		_information_events.flags.gps_checks_passed = true;
+		_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));
-		ECL_INFO("GPS checks passed");
-	}
 
-	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
+		_information_events.flags.gps_checks_passed = true;
-		// a rough 2D fix is sufficient to lookup declination
+
+		ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
+			 _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
+
+	} else {
+		// a rough 2D fix is sufficient to lookup earth spin rate
 		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
 
 		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
-			updateWmm(gps.lat, gps.lon);
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
 		}
-
-		_wmm_gps_time_last_checked = _time_delayed_us;
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -39,6 +39,8 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+#include <lib/world_magnetic_model/geo_mag_declination.h>
+
 #include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 
 void Ekf::controlMagFusion(const imuSample &imu_sample)
@@ -76,15 +78,43 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_mag_counter++;
 		}
 
+		// check for WMM update periodically or if global origin has changed
+		bool wmm_updated = false;
+
+		if (global_origin().isInitialized()) {
+
+			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
+
+			if (global_origin_valid()
+			    && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
+			   ) {
+				// position of local NED origin in GPS / WGS84 frame
+				double latitude_deg;
+				double longitude_deg;
+				global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
+
+				if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
+					wmm_updated = true;
+				}
+
+				_wmm_mag_time_last_checked = _time_delayed_us;
+
+			} else if (origin_newer_than_last_mag) {
+				// use global origin to update WMM
+				if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),
+							     global_origin().getProjectionReferenceLon())
+				   ) {
+					wmm_updated = true;
+				}
+			}
+		}
+
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
-		    && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
+		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
 		   ) {
-			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
+			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
-							* Vector3f(_mag_strength_gps, 0, 0);
-
-			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
 
 			_control_status.flags.synthetic_mag_z = true;
 
@@ -140,8 +170,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		checkMagHeadingConsistency(mag_sample);
 
-		// WMM update can occur on the last epoch, just after mag fusion
-		const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
 		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
 
 
@@ -203,7 +231,21 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					}
 
 					if (_control_status.flags.mag_dec) {
-						fuseDeclination(0.5f);
+
+						if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+						    && PX4_ISFINITE(_wmm_declination_rad)
+						   ) {
+							fuseDeclination(_wmm_declination_rad, 0.5f);
+
+						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+							  ) {
+
+							fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f);
+
+						} else {
+							_control_status.flags.mag_dec = false;
+						}
 					}
 				}
 
@@ -333,24 +375,21 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	resetMagCov();
 
 	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+	if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
-
 		// use expected earth field to reset states
-		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
-						* Vector3f(_mag_strength_gps, 0, 0);
 
 		// mag_B: reset
 		if (!reset_heading && _control_status.flags.yaw_align) {
 			// mag_B: reset using WMM
 			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-			_state.mag_B = mag - (R_to_body * mag_earth_pred);
+			_state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss);
 
 		} else {
 			_state.mag_B.zero();
 		}
 
 		// mag_I: reset, skipped if no change in state and variance good
-		_state.mag_I = mag_earth_pred;
+		_state.mag_I = _wmm_earth_field_gauss;
 
 		if (reset_heading) {
 			resetMagHeading(mag);
@@ -451,8 +490,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	_mag_strength = mag_sample.length();
 
 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
-		if (PX4_ISFINITE(_mag_strength_gps)) {
+		if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
-			if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
@@ -475,9 +514,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
 
 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
-		if (PX4_ISFINITE(_mag_inclination_gps)) {
+		if (PX4_ISFINITE(_wmm_inclination_rad)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
-			const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
+			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
 
 			if (fabsf(inc_error_rad) > inc_tol_rad) {
 				_control_status.flags.mag_field_disturbed = true;
@@ -549,13 +588,60 @@ float Ekf::getMagDeclination()
 		// Use value consistent with earth field state
 		return atan2f(_state.mag_I(1), _state.mag_I(0));
 
-	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
-		// use parameter value until GPS is available, then use value returned by geo library
+		   && PX4_ISFINITE(_wmm_declination_rad)
-		if (PX4_ISFINITE(_mag_declination_gps)) {
+		  ) {
-			return _mag_declination_gps;
+		// if available use value returned by geo library
+		return _wmm_declination_rad;
+
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
+		// using saved mag declination
+		return math::radians(_params.mag_declination_deg);
+	}
+
+	// otherwise unavailable
+	return 0.f;
+}
+
+bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
+{
+	// set the magnetic field data returned by the geo library using the current GPS position
+	const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
+	const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
+	const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);
+
+	if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) {
+
+		const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f));
+		const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f));
+		const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f);
+
+		if (!PX4_ISFINITE(_wmm_declination_rad)
+		    || !PX4_ISFINITE(_wmm_inclination_rad)
+		    || !PX4_ISFINITE(_wmm_field_strength_gauss)
+		    || !_wmm_earth_field_gauss.longerThan(0.f)
+		    || !_wmm_earth_field_gauss.isAllFinite()
+		    || declination_changed
+		    || inclination_changed
+		    || strength_changed
+		   ) {
+
+			ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
+				  (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
+				  (double)latitude_deg, (double)longitude_deg
+				 );
+
+			_wmm_declination_rad = declination_rad;
+			_wmm_inclination_rad = inclination_rad;
+			_wmm_field_strength_gauss = strength_gauss;
+
+			_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0);
+
+			return true;
 		}
 	}
 
-	// otherwise use the parameter value
+	return false;
-	return math::radians(_params.mag_declination_deg);
 }

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -150,51 +150,34 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }
 
-bool Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
 {
-	float decl_measurement = NAN;
+	// observation variance (rad**2)
+	const float R_DECL = sq(decl_sigma);
 
-	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	VectorState H;
-	    && PX4_ISFINITE(_mag_declination_gps)
+	float decl_pred;
-	   ) {
+	float innovation_variance;
-		decl_measurement = _mag_declination_gps;
 
-	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON,
-		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+			&decl_pred, &innovation_variance, &H);
-		  ) {
+
-		decl_measurement = math::radians(_params.mag_declination_deg);
+	const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
+
+	if (innovation_variance < R_DECL) {
+		// variance calculation is badly conditioned
+		_fault_status.flags.bad_mag_decl = true;
+		return false;
 	}
 
-	if (PX4_ISFINITE(decl_measurement)) {
+	// Calculate the Kalman gains
+	VectorState Kfusion = P * H / innovation_variance;
 
-		// observation variance (rad**2)
+	const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
-		const float R_DECL = sq(decl_sigma);
 
-		VectorState H;
+	_fault_status.flags.bad_mag_decl = !is_fused;
-		float decl_pred;
-		float innovation_variance;
 
-		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
+	return is_fused;
-				&H);
-
-		const float innovation = wrap_pi(decl_pred - decl_measurement);
-
-		if (innovation_variance < R_DECL) {
-			// variance calculation is badly conditioned
-			return false;
-		}
-
-		// Calculate the Kalman gains
-		VectorState Kfusion = P * H / innovation_variance;
-
-		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
-
-		_fault_status.flags.bad_mag_decl = !is_fused;
-
-		return is_fused;
-	}
-
-	return false;
 }
 
 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)

src/modules/ekf2/EKF/ekf.h

@@ -259,7 +259,6 @@ public:
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
-	void updateWmm(double lat, double lon);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -466,6 +465,9 @@ public:
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+	// set the magnetic field data returned by the geo library using position
+	bool updateWorldMagneticModel(const double latitude_deg, const double longitude_deg);
+
 	const auto &aid_src_mag() const { return _aid_src_mag; }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -770,8 +772,8 @@ private:
 		     bool update_all_states = false, bool update_tilt = false);
 
 	// fuse magnetometer declination measurement
-	// argument passed in is the declination uncertainty in radians
+	//  declination uncertainty in radians
-	bool fuseDeclination(float decl_sigma);
+	bool fuseDeclination(float decl_measurement_rad, float decl_sigma);
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -96,13 +96,13 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 		_gps_alt_ref = altitude;
 
-		updateWmm(current_lat, current_lon);
-
 		_gpos_origin_eph = eph;
 		_gpos_origin_epv = epv;
 
 		_NED_origin_initialised = true;
 
+		_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(latitude)));
+
 		if (current_pos_available) {
 			// reset horizontal position if we already have a global origin
 			Vector2f position = _pos_ref.project(current_lat, current_lon);
@@ -131,39 +131,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 	return false;
 }
 
-void Ekf::updateWmm(const double lat, const double lon)
-{
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-
-	// set the magnetic field data returned by the geo library using the current GPS position
-	const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon));
-	const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon));
-	const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
-
-	if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
-
-		const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
-		const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
-
-		if ((_wmm_gps_time_last_set == 0)
-		    || !PX4_ISFINITE(_mag_declination_gps)
-		    || !PX4_ISFINITE(_mag_inclination_gps)
-		    || !PX4_ISFINITE(_mag_strength_gps)
-		    || mag_declination_changed
-		    || mag_inclination_changed
-		   ) {
-			_mag_declination_gps = mag_declination_gps;
-			_mag_inclination_gps = mag_inclination_gps;
-			_mag_strength_gps = mag_strength_gps;
-
-			_wmm_gps_time_last_set = _time_delayed_us;
-		}
-	}
-
-#endif // CONFIG_EKF2_MAGNETOMETER
-}
-
-
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	float eph = INFINITY;

src/modules/ekf2/EKF/estimator_interface.h

@@ -250,7 +250,7 @@ public:
 	bool get_mag_decl_deg(float &val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			val = math::degrees(_mag_declination_gps);
+			val = math::degrees(_wmm_declination_rad);
 			return true;
 
 		} else {
@@ -261,7 +261,7 @@ public:
 	bool get_mag_inc_deg(float &val) const
 	{
 		if (_NED_origin_initialised) {
-			val = math::degrees(_mag_inclination_gps);
+			val = math::degrees(_wmm_inclination_rad);
 			return true;
 
 		} else {
@@ -272,9 +272,9 @@ public:
 	void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
 	{
 		inc_deg = math::degrees(_mag_inclination);
-		inc_ref_deg = math::degrees(_mag_inclination_gps);
+		inc_ref_deg = math::degrees(_wmm_inclination_rad);
 		strength_gs = _mag_strength;
-		strength_ref_gs = _mag_strength_gps;
+		strength_ref_gs = _wmm_field_strength_gauss;
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -451,13 +451,14 @@ protected:
 	// allocate data buffers and initialize interface variables
 	bool initialise_interface(uint64_t timestamp);
 
-	uint64_t _wmm_gps_time_last_checked{0};  // time WMM last checked
-	uint64_t _wmm_gps_time_last_set{0};      // time WMM last set
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	float _mag_declination_gps {NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
-	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+
-	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
+	float _wmm_declination_rad{NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _wmm_inclination_rad{NAN};        // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _wmm_field_strength_gauss{NAN};   // magnetic strength returned by the geo library using the last valid GPS position (Gauss)
+
+	Vector3f _wmm_earth_field_gauss{};      // expected magnetic field vector from the last valid GPS position (Gauss)
 
 	float _mag_inclination{NAN};
 	float _mag_strength{NAN};

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -199,7 +199,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
 
 	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
 	const Vector3f vel = _ekf->getVelocity();
-	EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-3f);
+	EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f);
 	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
 	EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
 	EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -108,7 +108,6 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag);
-	EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_dec);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);