ekf2: cleanup more optional mag (CONFIG_EKF2_MAGNETOMETER)

src/modules/ekf2/EKF/common.h

@@ -91,6 +91,7 @@ enum class VelocityFrame : uint8_t {
 	BODY_FRAME_FRD  = 2
 };
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 enum GeoDeclinationMask : uint8_t {
 	// Bit locations for mag_declination_source
 	USE_GEO_DECL  = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
@@ -104,6 +105,7 @@ enum MagFuseType : uint8_t {
 	HEADING = 1,   	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
 	NONE    = 5    	///< Do not use magnetometer under any circumstance..
 };
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_TERRAIN)
 enum TerrainFusionMask : uint8_t {
@@ -284,7 +286,6 @@ struct parameters {
 	int32_t sensor_interval_max_ms{10};     ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
 
 	// measurement time delays
-	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
 	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
 	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
 
@@ -295,8 +296,6 @@ struct parameters {
 	// process noise
 	float gyro_bias_p_noise{1.0e-3f};       ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
 	float accel_bias_p_noise{1.0e-2f};      ///< process noise for IMU accelerometer bias prediction (m/sec**3)
-	float mage_p_noise{1.0e-3f};            ///< process noise for earth magnetic field prediction (Gauss/sec)
-	float magb_p_noise{1.0e-4f};            ///< process noise for body magnetic field prediction (Gauss/sec)
 
 #if defined(CONFIG_EKF2_WIND)
 	const float initial_wind_uncertainty{1.0f};     ///< 1-sigma initial uncertainty in wind velocity (m/sec)
@@ -322,17 +321,31 @@ struct parameters {
 	float gnd_effect_deadzone{5.0f};        ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
 	float gnd_effect_max_hgt{0.5f};         ///< Height above ground at which baro ground effect becomes insignificant (m)
 
-	// magnetometer fusion
+	float heading_innov_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
 	float mag_heading_noise{3.0e-1f};       ///< measurement noise used for simple heading fusion (rad)
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
+
+	float mage_p_noise{1.0e-3f};            ///< process noise for earth magnetic field prediction (Gauss/sec)
+	float magb_p_noise{1.0e-4f};            ///< process noise for body magnetic field prediction (Gauss/sec)
+
+	// magnetometer fusion
 	float mag_noise{5.0e-2f};               ///< measurement noise used for 3-axis magnetometer fusion (Gauss)
 	float mag_declination_deg{0.0f};        ///< magnetic declination (degrees)
-	float heading_innov_gate{2.6f};         ///< heading fusion innovation consistency gate size (STD)
 	float mag_innov_gate{3.0f};             ///< magnetometer fusion innovation consistency gate size (STD)
 	int32_t mag_declination_source{7};      ///< bitmask used to control the handling of declination data
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
 	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)
 
+	// compute synthetic magnetomter Z value if possible
+	int32_t synthesize_mag_z{0};
+	int32_t mag_check{0};
+	float mag_check_strength_tolerance_gs{0.2f};
+	float mag_check_inclination_tolerance_deg{20.f};
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
@@ -483,12 +496,6 @@ struct parameters {
 	const float auxvel_gate{5.0f};          ///< velocity fusion innovation consistency gate size (STD)
 #endif // CONFIG_EKF2_AUXVEL
 
-	// compute synthetic magnetomter Z value if possible
-	int32_t synthesize_mag_z{0};
-	int32_t mag_check{0};
-	float mag_check_strength_tolerance_gs{0.2f};
-	float mag_check_inclination_tolerance_deg{20.f};
-
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value

src/modules/ekf2/EKF/covariance.cpp

@@ -177,24 +177,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		}
 	}
 
-	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_I_sig = 0.0f;
-
-	if (_control_status.flags.mag && P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
-#endif // CONFIG_EKF2_MAGNETOMETER
-	}
-
-	// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_B_sig = 0.0f;
-
-	if (_control_status.flags.mag && P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
-#endif // CONFIG_EKF2_MAGNETOMETER
-	}
-
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
 	float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
@@ -255,18 +237,31 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 
 	if (_control_status.flags.mag) {
-		const float mag_I_process_noise = sq(mag_I_sig);
-		for (unsigned index = 0; index < State::mag_I.dof; index++) {
-			unsigned i = State::mag_I.idx + index;
-			nextP(i, i) += mag_I_process_noise;
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+		// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
+		if (P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
+
+			float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
+			float mag_I_process_noise = sq(mag_I_sig);
+
+			for (unsigned index = 0; index < State::mag_I.dof; index++) {
+				unsigned i = State::mag_I.idx + index;
+				nextP(i, i) += mag_I_process_noise;
+			}
 		}
 
-		const float mag_B_process_noise = sq(mag_B_sig);
-		for (unsigned index = 0; index < State::mag_B.dof; index++) {
-			unsigned i = State::mag_B.idx + index;
-			nextP(i, i) += mag_B_process_noise;
-		}
+		// Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
+		if (P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
 
+			float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
+			float mag_B_process_noise = sq(mag_B_sig);
+
+			for (unsigned index = 0; index < State::mag_B.dof; index++) {
+				unsigned i = State::mag_B.idx + index;
+				nextP(i, i) += mag_B_process_noise;
+			}
+		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 	} else {
 		// keep previous covariance
 		for (unsigned i = 0; i < State::mag_I.dof; i++) {
@@ -335,8 +330,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	const float vel_var_max = 1e6f;
 	const float pos_var_max = 1e6f;
 	const float gyro_bias_var_max = 1.0f;
-	const float mag_I_var_max = 1.0f;
-	const float mag_B_var_max = 1.0f;
 
 	constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
 	constrainStateVar(State::vel, 1e-6f, vel_var_max);
@@ -471,17 +464,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 
 	// magnetic field states
 	if (!_control_status.flags.mag) {
-		P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
-		P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
+		P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.0f);
+		P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.0f);
 
 	} else {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+		const float mag_I_var_max = 1.f;
 		constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
+
+		const float mag_B_var_max = 1.f;
 		constrainStateVar(State::mag_B, 0.f, mag_B_var_max);
 
 		if (force_symmetry) {
 			P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
 			P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
 	// wind velocity states

src/modules/ekf2/EKF/ekf.cpp

@@ -57,14 +57,18 @@ void Ekf::reset()
 {
 	ECL_INFO("reset");
 
+	_state.quat_nominal.setIdentity();
 	_state.vel.setZero();
 	_state.pos.setZero();
 	_state.gyro_bias.setZero();
 	_state.accel_bias.setZero();
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_state.mag_I.setZero();
 	_state.mag_B.setZero();
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	_state.wind_vel.setZero();
-	_state.quat_nominal.setIdentity();
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -239,8 +239,8 @@ void Ekf::constrainStates()
 	const float accel_bias_limit = getAccelBiasLimit();
 	_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
 
-	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
 	_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -401,6 +401,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 		_gps_alt_ref = altitude;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
 		const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
 		const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
@@ -412,6 +413,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 			_wmm_gps_time_last_set = _time_delayed_us;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 		// We don't know the uncertainty of the origin
 		_gpos_origin_eph = 0.f;
@@ -809,8 +811,12 @@ void Ekf::fuse(const VectorState &K, float innovation)
 	_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
 	_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
 	_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
 	_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
 }
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -571,10 +571,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	}
 #endif // CONFIG_EKF2_AIRSPEED
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// mag mode
 	if (_params.mag_fusion_type != MagFuseType::NONE) {
 		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// using range finder

src/modules/ekf2/EKF/estimator_interface.h

@@ -452,12 +452,15 @@ protected:
 
 	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)
 	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 _mag_inclination{NAN};
 	float _mag_strength{NAN};
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// this is the current status of the filter control modes
 	filter_control_status_u _control_status{};

src/modules/ekf2/EKF/gps_checks.cpp

@@ -41,7 +41,10 @@
 
 #include "ekf.h"
 
-#include <lib/world_magnetic_model/geo_mag_declination.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
@@ -100,6 +103,8 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 
 			// If we have good GPS data set the origin's WGS-84 position to the last gps fix
 			const double lat = gps.lat * 1.0e-7;
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 			const double lon = gps.lon * 1.0e-7;
 
 			// set the magnetic field data returned by the geo library using the current GPS position
@@ -126,6 +131,7 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 					_wmm_gps_time_last_set = _time_delayed_us;
 				}
 			}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
 		}