implemented synthesized magnetometer Z measurement

- calculate a theoretical value based on the knowledge of the direction
and strength of the magnetic field vector and X/Y sensor measurements
- needs knowledge about location on earth to work

EKF/common.h

@@ -357,6 +357,9 @@ struct parameters {
 
 	// control of on-ground movement check
 	float is_moving_scaler{1.0f};		///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive.
+
+	// compute synthetic magnetomter Z value if possible
+	int32_t synthesize_mag_z{0};
 };
 
 struct stateSample {
@@ -458,6 +461,7 @@ union filter_control_status_u {
 		uint32_t gps_yaw     : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 		uint32_t mag_align_complete   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
 		uint32_t ev_vel      : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused
+		uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
 	} flags;
 	uint32_t value;
 };

EKF/control.cpp

@@ -90,6 +90,18 @@ void Ekf::controlFusionModes()
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
 
+	if (_mag_data_ready) {
+		// 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 & MASK_USE_GEO_DECL) &&_NED_origin_initialised) {
+			Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+			_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
+			_control_status.flags.synthetic_mag_z = true;
+		} else {
+			_control_status.flags.synthetic_mag_z = false;
+		}
+	}
+
 	_delta_time_baro_us = _baro_sample_delayed.time_us;
 	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
 

EKF/ekf.h

@@ -362,6 +362,7 @@ private:
 	bool _vehicle_at_rest_prev{false};	///< true when the vehicle was at rest the previous time the status was checked
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
+	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 
@@ -719,4 +720,8 @@ private:
 	// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
 	float kahanSummation(float sum_previous, float input, float &accumulator) const;
 
+	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
+	// sensor measurement
+	float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted);
+
 };

EKF/mag_fusion.cpp

@@ -81,8 +81,13 @@ void Ekf::fuseMag()
 	// compute magnetometer innovations
 	_mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0);
 	_mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1);
-	_mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2);
 
+	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
+	if (_control_status.flags.synthetic_mag_z) {
+		_mag_innov[2] = 0.0f;
+	} else {
+		_mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2);
+	}
 	// Observation jacobian and Kalman gain vectors
 	float H_MAG[24];
 	float Kfusion[24];
@@ -284,6 +289,12 @@ void Ekf::fuseMag()
 			Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
 
 		} else if (index == 2) {
+
+			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
+			if (_control_status.flags.synthetic_mag_z) {
+				continue;
+			}
+
 			// calculate Z axis observation jacobians
 			memset(H_MAG, 0, sizeof(H_MAG));
 			H_MAG[0] = SH_MAG[1];
@@ -983,3 +994,15 @@ void Ekf::limitDeclination()
 		_state.mag_I(1) = h_field * sinf(decl_min);
 	}
 }
+
+float Ekf::calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted)
+{
+	// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
+	// of the earth magnetic field vector at the current location
+	float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
+
+	// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetomer Z component
+	float mag_z_body_pred = _R_to_earth(0,2) * mag_earth_predicted(0) + _R_to_earth(1,2) * mag_earth_predicted(1) + _R_to_earth(2,2) * mag_earth_predicted(2);
+
+	return mag_z_body_pred < 0 ? -mag_z_abs : mag_z_abs;
+}