Merge pull request #55 from PX4/pr-FixYawFusionBug

Fix bug in heading fusion and add new method for ground/indoor use

EKF/common.h

@@ -163,8 +163,9 @@ struct parameters {
 
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
-#define MAG_FUSE_TYPE_HEADING   1   // Magnetic heading fusion will alays be used. This is less accurate, but less affected by earth field distortions
+#define MAG_FUSE_TYPE_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
 #define MAG_FUSE_TYPE_3D        2   // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
+#define MAG_FUSE_TYPE_2D        3   // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
 
 struct stateSample {
 	Vector3f    ang_error;	// attitude axis angle error (error state formulation)
@@ -215,12 +216,13 @@ union filter_control_status_u {
 		uint8_t yaw_align   : 1; // 1 - true if the filter yaw alignment is complete
 		uint8_t gps         : 1; // 2 - true if GPS measurements are being fused
 		uint8_t opt_flow    : 1; // 3 - true if optical flow measurements are being fused
-		uint8_t mag_hdg     : 1; // 4 - true if a simple magnetic heading is being fused
+		uint8_t mag_hdg     : 1; // 4 - true if a simple magnetic yaw heading is being fused
-		uint8_t mag_3D      : 1; // 5 - true if 3-axis magnetometer measurement are being fused
+		uint8_t mag_2D      : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
-		uint8_t mag_dec     : 1; // 6 - true if synthetic magnetic declination measurements are being fused
+		uint8_t mag_3D      : 1; // 6 - true if 3-axis magnetometer measurement are being fused
-		uint8_t in_air      : 1; // 7 - true when the vehicle is airborne
+		uint8_t mag_dec     : 1; // 7 - true if synthetic magnetic declination measurements are being fused
-		uint8_t armed       : 1; // 8 - true when the vehicle motors are armed
+		uint8_t in_air      : 1; // 8 - true when the vehicle is airborne
-		uint8_t wind        : 1; // 9 - true when wind velocity is being estimated
+		uint8_t armed       : 1; // 9 - true when the vehicle motors are armed
+		uint8_t wind        : 1; // 10 - true when wind velocity is being estimated
 	} flags;
 	uint16_t value;
 };

EKF/control.cpp

@@ -103,36 +103,55 @@ void Ekf::controlFusionModes()
 	// or the more accurate 3-axis fusion
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
 		if (!_control_status.flags.armed) {
-			// always use simple mag fusion for initial startup
+			// always use 2D mag fusion for initial startup
-			_control_status.flags.mag_hdg = true;
+			_control_status.flags.mag_hdg = false;
+			_control_status.flags.mag_2D = true;
 			_control_status.flags.mag_3D = false;
 
 		} else {
 			if (_control_status.flags.in_air) {
-				// always use 3-axis mag fusion when airborne
+				// always use 3D mag fusion when airborne
 				_control_status.flags.mag_hdg = false;
+				_control_status.flags.mag_2D = false;
 				_control_status.flags.mag_3D = true;
 
 			} else {
-				// always use simple heading fusion when on the ground
+				// always use 2D mag fusion when on the ground
-				_control_status.flags.mag_hdg = true;
+				_control_status.flags.mag_hdg = false;
+				_control_status.flags.mag_2D = true;
 				_control_status.flags.mag_3D = false;
 			}
 		}
 
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
-		// always use simple heading fusion
+		// always use yaw fusion unless tilt is over 45 deg, otherwise use 2D fusion
-		_control_status.flags.mag_hdg = true;
+		if (_R_prev(2, 2) > 0.7071f) {
+			_control_status.flags.mag_hdg = true;
+			_control_status.flags.mag_2D = false;
+
+		} else {
+			_control_status.flags.mag_hdg = false;
+			_control_status.flags.mag_2D = true;
+		}
+
+		_control_status.flags.mag_3D = false;
+
+	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_2D) {
+		// always use 2D mag fusion
+		_control_status.flags.mag_hdg = false;
+		_control_status.flags.mag_2D = true;
 		_control_status.flags.mag_3D = false;
 
 	} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
 		// always use 3-axis mag fusion
 		_control_status.flags.mag_hdg = false;
+		_control_status.flags.mag_2D = false;
 		_control_status.flags.mag_3D = true;
 
 	} else {
 		// do no magnetometer fusion at all
 		_control_status.flags.mag_hdg = false;
+		_control_status.flags.mag_2D = false;
 		_control_status.flags.mag_3D = false;
 	}
 

EKF/ekf.cpp

@@ -150,6 +150,9 @@ bool Ekf::update()
 				fuseDeclination();
 			}
 
+		} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
+			fuseMag2D();
+
 		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
 			fuseHeading();
 

EKF/ekf.h

@@ -183,9 +183,12 @@ private:
 	// ekf sequential fusion of magnetometer measurements
 	void fuseMag();
 
-	// fuse magnetometer heading measurement
+	// fuse heading measurement (currently uses estimate from magnetometer)
 	void fuseHeading();
 
+	// fuse projecton of magnetometer onto horizontal plane
+	void fuseMag2D();
+
 	// fuse magnetometer declination measurement
 	void fuseDeclination();
 

EKF/mag_fusion.cpp

@@ -495,7 +495,7 @@ void Ekf::fuseHeading()
 	float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
 	R_YAW = R_YAW * R_YAW;
 
-	// calculate intermediate variables for observation jacobian
+	// calculate intermediate variables for observation jacobians
 	float t2 = q0 * q0;
 	float t3 = q1 * q1;
 	float t4 = q2 * q2;
@@ -509,7 +509,7 @@ void Ekf::fuseHeading()
 	if (fabsf(t6) > 1e-6f) {
 		t10 = 1.0f / (t6 * t6);
 
-	} else  {
+	} else {
 		return;
 	}
 
@@ -527,36 +527,39 @@ void Ekf::fuseHeading()
 
 	float t15 = 1.0f / t6;
 
-	// calculate observation jacobian
 	float H_YAW[3] = {};
 	H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f));
-	H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));
+	H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));	// calculate observation jacobian
 
-	// calculate innovation
+	// calculate intermediate expressions for Kalman gains
-	// rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement
+	float t16 = q0 * q1 * 2.0f;
-	// TODO - enable use of an off-board heading measurement
+	float t29 = q2 * q3 * 2.0f;
-	matrix::Dcm<float> R_to_earth(_state.quat_nominal);
+	float t17 = t16 - t29;
-	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+	float t18 = t15 * t17;
-	float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
+	float t19 = q0 * q2 * 2.0f;
+	float t20 = q1 * q3 * 2.0f;
+	float t21 = t19 + t20;
+	float t22 = t9 * t10 * t21;
+	float t23 = t18 + t22;
+	float t40 = t14 * t23;
+	float t24 = t2 - t3 + t4 - t5;
+	float t25 = t15 * t24;
+	float t26 = t7 - t8;
+	float t27 = t9 * t10 * t26;
+	float t28 = t25 + t27;
+	float t41 = t14 * t28;
+	float t30 = P[1][1] * t40;
+	float t31 = P[1][2] * t40;
+	float t32 = P[2][2] * t41;
+	float t33 = t31 + t32;
+	float t34 = t41 * t33;
+	float t35 = P[2][1] * t41;
+	float t36 = t30 + t35;
+	float t37 = t40 * t36;
+	float t38 = R_YAW + t34 + t37; // Innovation variance
+	_heading_innov_var = t38;
 
-	// wrap the innovation to the interval between +-pi
+	if (t38 >= R_YAW) {
-	innovation = matrix::wrap_pi(innovation);
-	_heading_innov = innovation;
-
-	// calculate innovation variance
-	float innovation_var = R_YAW;
-	_heading_innov_var = innovation_var;
-	float PH[3] = {};
-
-	for (unsigned row = 0; row < 3; row++) {
-		for (unsigned column = 0; column < 3; column++) {
-			PH[row] += P[row][column] * H_YAW[column];
-		}
-
-		innovation_var += H_YAW[row] * PH[row];
-	}
-
-	if (innovation_var >= R_YAW) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.bad_mag_hdg = false;
 
@@ -569,44 +572,65 @@ void Ekf::fuseHeading()
 		return;
 	}
 
-	float innovation_var_inv = 1.0f / innovation_var;
+	float t39 = 1.0f / t38;
 
-	// calculate the kalman gains taking dvantage of the reduce size of H_YAW
+	// calculate Kalman gains
-	float Kfusion[_k_num_states] = {};
+	float Kfusion[24] = {};
+	Kfusion[0] = t39 * (P[0][1] * t40 + P[0][2] * t41);
+	Kfusion[1] = t39 * (t30 + P[1][2] * t41);
+	Kfusion[2] = t39 * (t32 + P[2][1] * t40);
+	Kfusion[3] = t39 * (P[3][1] * t40 + P[3][2] * t41);
+	Kfusion[4] = t39 * (P[4][1] * t40 + P[4][2] * t41);
+	Kfusion[5] = t39 * (P[5][1] * t40 + P[5][2] * t41);
+	Kfusion[6] = t39 * (P[6][1] * t40 + P[6][2] * t41);
+	Kfusion[7] = t39 * (P[7][1] * t40 + P[7][2] * t41);
+	Kfusion[8] = t39 * (P[8][1] * t40 + P[8][2] * t41);
+	Kfusion[9] = t39 * (P[9][1] * t40 + P[9][2] * t41);
+	Kfusion[10] = t39 * (P[10][1] * t40 + P[10][2] * t41);
+	Kfusion[11] = t39 * (P[11][1] * t40 + P[11][2] * t41);
+	Kfusion[12] = t39 * (P[12][1] * t40 + P[12][2] * t41);
+	Kfusion[13] = t39 * (P[13][1] * t40 + P[13][2] * t41);
+	Kfusion[14] = t39 * (P[14][1] * t40 + P[14][2] * t41);
+	Kfusion[15] = t39 * (P[15][1] * t40 + P[15][2] * t41);
 
-	// gains for states that are always used
+	/* we won't be using these states because we are doing heading fusion
-	for (unsigned row = 0; row <= 15; row++) {
+	Kfusion[16] = t39*(P[16][1]*t40+P[16][2]*t41);
-		for (unsigned column = 0; column < 3; column++) {
+	Kfusion[17] = t39*(P[17][1]*t40+P[17][2]*t41);
-			Kfusion[row] += P[row][column] * H_YAW[column];
+	Kfusion[18] = t39*(P[18][1]*t40+P[18][2]*t41);
-		}
+	Kfusion[19] = t39*(P[19][1]*t40+P[19][2]*t41);
+	Kfusion[20] = t39*(P[20][1]*t40+P[20][2]*t41);
+	Kfusion[21] = t39*(P[21][1]*t40+P[21][2]*t41);
+	*/
 
-		Kfusion[row] *= innovation_var_inv;
+	// don't adjust these states if we are not using them
-	}
-
-	// only calculate gains for magnetic field states if they are being used
-	if (_control_status.flags.mag_3D) {
-		for (unsigned row = 16; row <= 21; row++) {
-			for (unsigned column = 0; column < 3; column++) {
-				Kfusion[row] += P[row][column] * H_YAW[column];
-			}
-
-			Kfusion[row] *= innovation_var_inv;
-		}
-	}
-
-	// only calculate gains for wind states if they are being used
 	if (_control_status.flags.wind) {
-		for (unsigned row = 22; row <= 23; row++) {
+		Kfusion[22] = t39 * (P[22][1] * t40 + P[22][2] * t41);
-			for (unsigned column = 0; column < 3; column++) {
+		Kfusion[23] = t39 * (P[23][1] * t40 + P[23][2] * t41);
-				Kfusion[row] += P[row][column] * H_YAW[column];
-			}
-
-			Kfusion[row] *= innovation_var_inv;
-		}
 	}
 
+	// TODO - enable use of an off-board heading measurement
+
+	// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
+	matrix::Euler<float> euler(_state.quat_nominal);
+	float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
+	euler(2) = 0.0f;
+	matrix::Dcm<float> R_to_earth(euler);
+	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+
+	// Use the difference between the horizontal projection and declination to give the measured heading
+	float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
+
+	// wrap the heading to the interval between +-pi
+	measured_hdg = matrix::wrap_pi(measured_hdg);
+
+	// calculate the innovation
+	_heading_innov = predicted_hdg - measured_hdg;
+
+	// wrap the innovation to the interval between +-pi
+	_heading_innov = matrix::wrap_pi(_heading_innov);
+
 	// innovation test ratio
-	_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
+	_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
 
 	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
@@ -620,8 +644,8 @@ void Ekf::fuseHeading()
 
 		} else {
 			// constrain the innovation to the maximum set by the gate
-			float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var));
+			float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
-			innovation = math::constrain(innovation, -gate_limit, gate_limit);
+			_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
 		}
 
 	} else {
@@ -630,7 +654,7 @@ void Ekf::fuseHeading()
 
 	// zero the attitude error states and use the kalman gain vector and innovation to update the states
 	_state.ang_error.setZero();
-	fuse(Kfusion, innovation);
+	fuse(Kfusion, _heading_innov);
 
 	// correct the nominal quaternion
 	Quaternion dq;
@@ -642,7 +666,7 @@ void Ekf::fuseHeading()
 	float HP[_k_num_states] = {};
 
 	for (unsigned column = 0; column < _k_num_states; column++) {
-		for (unsigned row = 0; row < 3; row++) {
+		for (unsigned row = 1; row <= 2; row++) {
 			HP[column] += H_YAW[row] * P[row][column];
 		}
 	}
@@ -776,8 +800,7 @@ void Ekf::fuseDeclination()
 
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 0; column < _k_num_states; column++) {
-			float tmp = KH[row][0] * P[0][column];
+			float tmp = KH[row][16] * P[16][column];
-			tmp += KH[row][16] * P[16][column];
 			tmp += KH[row][17] * P[17][column];
 			KHP[row][column] = tmp;
 		}
@@ -793,3 +816,194 @@ void Ekf::fuseDeclination()
 	makeSymmetrical();
 	limitCov();
 }
+
+void Ekf::fuseMag2D()
+{
+	// assign intermediate state variables
+	float q0 = _state.quat_nominal(0);
+	float q1 = _state.quat_nominal(1);
+	float q2 = _state.quat_nominal(2);
+	float q3 = _state.quat_nominal(3);
+
+	float magX = _mag_sample_delayed.mag(0);
+	float magY = _mag_sample_delayed.mag(1);
+	float magZ = _mag_sample_delayed.mag(2);
+
+	float R_DECL = fmaxf(_params.mag_heading_noise, 1.0e-2f);
+	R_DECL = R_DECL * R_DECL;
+
+	// calculate intermediate variables for observation jacobian
+	float t2 = q0 * q0;
+	float t3 = q1 * q1;
+	float t4 = q2 * q2;
+	float t5 = q3 * q3;
+	float t6 = q0 * q3 * 2.0f;
+	float t8 = t2 - t3 + t4 - t5;
+	float t9 = q0 * q1 * 2.0f;
+	float t10 = q2 * q3 * 2.0f;
+	float t11 = t9 - t10;
+	float t14 = q1 * q2 * 2.0f;
+	float t21 = magY * t8;
+	float t22 = t6 + t14;
+	float t23 = magX * t22;
+	float t24 = magZ * t11;
+	float t7 = t21 + t23 - t24;
+	float t12 = t2 + t3 - t4 - t5;
+	float t13 = magX * t12;
+	float t15 = q0 * q2 * 2.0f;
+	float t16 = q1 * q3 * 2.0f;
+	float t17 = t15 + t16;
+	float t18 = magZ * t17;
+	float t19 = t6 - t14;
+	float t25 = magY * t19;
+	float t20 = t13 + t18 - t25;
+
+	if (fabsf(t20) < 1e-6f) {
+		return;
+	}
+
+	float t26 = 1.0f / (t20 * t20);
+	float t27 = t7 * t7;
+	float t28 = t26 * t27;
+	float t29 = t28 + 1.0f;
+
+	if (fabsf(t29) < 1e-12f) {
+		return;
+	}
+
+	float t30 = 1.0f / t29;
+
+	if (fabsf(t20) < 1e-12f) {
+		return;
+	}
+
+	float t31 = 1.0f / t20;
+
+	// calculate observation jacobian
+	float H_DECL[3] = {};
+	H_DECL[0] = -t30 * (t31 * (magZ * t8 + magY * t11) + t7 * t26 * (magY * t17 + magZ * t19));
+	H_DECL[1] = t30 * (t31 * (magX * t11 + magZ * t22) - t7 * t26 * (magZ * t12 - magX * t17));
+	H_DECL[2] = t30 * (t31 * (magX * t8 - magY * t22) + t7 * t26 * (magY * t12 + magX * t19));
+
+	// rotate the magnetometer measurement into earth frame
+	matrix::Dcm<float> R_to_earth(_state.quat_nominal);
+	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+
+	// check if there is enough magnetic field length to use and exit if too small
+	float magLength2 = sq(mag_earth_pred(0) + mag_earth_pred(1));
+
+	if (magLength2 < sq(_params.mag_noise)) {
+		return;
+	}
+
+	// Adjust the measurement variance upwards if thehorizontal strength to magnetometer noise ratio make the value unrealistic
+	R_DECL = fmaxf(R_DECL, sq(_params.mag_noise) / magLength2);
+
+	// Calculate the innovation, using the declination angle of the projection onto the horizontal as the measurement
+	_heading_innov = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
+
+	// wrap the innovation to the interval between +-pi
+	_heading_innov = matrix::wrap_pi(_heading_innov);
+
+	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
+	float PH[3];
+	_heading_innov_var = R_DECL;
+
+	for (unsigned row = 0; row <= 2; row++) {
+		PH[row] = 0.0f;
+
+		for (unsigned col = 0; col <= 2; col++) {
+			PH[row] += P[row][col] * H_DECL[col];
+		}
+
+		_heading_innov_var += H_DECL[row] * PH[row];
+	}
+
+	float varInnovInv;
+
+	if (_heading_innov_var >= R_DECL) {
+		// the innovation variance contribution from the state covariances is not negative, no fault
+		_fault_status.bad_mag_hdg = false;
+
+	} else {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		_fault_status.bad_mag_hdg = true;
+
+		// we reinitialise the covariance matrix and abort this fusion step
+		initialiseCovariance();
+		return;
+	}
+
+	// innovation test ratio
+	_yaw_test_ratio = sq(_heading_innov) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var);
+
+	// set the magnetometer unhealthy if the test fails
+	if (_yaw_test_ratio > 1.0f) {
+		_mag_healthy = false;
+
+		// if we are in air we don't want to fuse the measurement
+		// we allow to use it when on the ground because the large innovation could be caused
+		// by interference or a large initial gyro bias
+		if (_control_status.flags.in_air) {
+			printf("return 5\n");
+			return;
+
+		} else {
+			// constrain the innovation to the maximum set by the gate
+			float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * _heading_innov_var));
+			_heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
+		}
+
+	} else {
+		_mag_healthy = true;
+	}
+
+	varInnovInv = 1.0f / _heading_innov_var;
+
+	// calculate the Kalman gains
+	float Kfusion[24] = {};
+
+	for (unsigned row = 0; row < 16; row++) {
+		Kfusion[row] = 0.0f;
+
+		for (unsigned col = 0; col <= 2; col++) {
+			Kfusion[row] += P[row][col] * H_DECL[col];
+		}
+
+		Kfusion[row] *= varInnovInv;
+	}
+
+	// by definition our error state is zero at the time of fusion
+	_state.ang_error.setZero();
+
+	// correct the states
+	fuse(Kfusion, _heading_innov);
+
+	// correct the quaternon using the attitude error estimate
+	Quaternion q_correction;
+	q_correction.from_axis_angle(_state.ang_error);
+	_state.quat_nominal = q_correction * _state.quat_nominal;
+	_state.quat_nominal.normalize();
+	_state.ang_error.setZero();
+
+	// correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
+	// and we only need the first 16 states
+	float HP[16];
+
+	for (uint8_t col = 0; col < 16; col++) {
+		HP[col] = 0.0f;
+
+		for (uint8_t row = 0; row <= 2; row++) {
+			HP[col] += H_DECL[row] * P[row][col];
+		}
+	}
+
+	for (uint8_t row = 0; row < 16; row++) {
+		for (uint8_t col = 0; col < 16; col++) {
+			P[row][col] -= Kfusion[row] * HP[col];
+		}
+	}
+
+	makeSymmetrical();
+	limitCov();
+}

matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt

@@ -1,32 +1,57 @@
 /* 
-Autocode for fusion of a yaw error measurement where the innovation is given by: 
+Observation jacobian for fusion of the horizontal components of magnetometer measurements.
 
-innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
+innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
+by rotating the measured magnetometer readings from body into earth axes.
 
-magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
+This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
-declinationAngle is the estimated declination as that location
 
-Protection against /0 and covariance matrix errors will need to be added.
+Divide by zero protection hs been added
 */
 
-// intermediate variables
+// calculate intermediate variables for observation jacobian
 float t2 = q0*q0;
 float t3 = q1*q1;
 float t4 = q2*q2;
 float t5 = q3*q3;
-float t6 = t2+t3-t4-t5;
+float t6 = q0*q3*2.0f;
-float t7 = q0*q3*2.0f;
+float t8 = t2-t3+t4-t5;
-float t8 = q1*q2*2.0f;
+float t9 = q0*q1*2.0f;
-float t9 = t7+t8;
+float t10 = q2*q3*2.0f;
-float t10 = 1.0f/(t6*t6);
+float t11 = t9-t10;
-float t11 = t9*t9;
+float t14 = q1*q2*2.0f;
-float t12 = t10*t11;
+float t21 = magY*t8;
-float t13 = t12+1.0f;
+float t22 = t6+t14;
-float t14 = 1.0f/t13;
+float t23 = magX*t22;
-float t15 = 1.0f/t6;
+float t24 = magZ*t11;
+float t7 = t21+t23-t24;
+float t12 = t2+t3-t4-t5;
+float t13 = magX*t12;
+float t15 = q0*q2*2.0f;
+float t16 = q1*q3*2.0f;
+float t17 = t15+t16;
+float t18 = magZ*t17;
+float t19 = t6-t14;
+float t25 = magY*t19;
+float t20 = t13+t18-t25;
+if (fabsf(t20) < 1e-6f) {
+    return;
+}
+float t26 = 1.0f/(t20*t20);
+float t27 = t7*t7;
+float t28 = t26*t27;
+float t29 = t28+1.0;
+if (fabsf(t29) < 1e-12f) {
+    return;
+}
+float t30 = 1.0f/t29;
+if (fabsf(t20) < 1e-12f) {
+    return;
+}
+float t31 = 1.0f/t20;
 
-// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
+// calculate observation jacobian
-// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
+float H_DECL[3] = {};
-float H_MAG[3];
+H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
-H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
+H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
-H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
+H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));

matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt

@@ -0,0 +1,27 @@
+/* 
+Autocode for fusion of an Euler yaw measurement.
+
+Protection against /0 and covariance matrix errors will need to be added.
+*/
+
+// intermediate variables
+float t2 = q0*q0;
+float t3 = q1*q1;
+float t4 = q2*q2;
+float t5 = q3*q3;
+float t6 = t2+t3-t4-t5;
+float t7 = q0*q3*2.0f;
+float t8 = q1*q2*2.0f;
+float t9 = t7+t8;
+float t10 = 1.0f/(t6*t6);
+float t11 = t9*t9;
+float t12 = t10*t11;
+float t13 = t12+1.0f;
+float t14 = 1.0f/t13;
+float t15 = 1.0f/t6;
+
+// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
+// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
+float H_MAG[3];
+H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
+H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));

matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m

@@ -68,8 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
 syms ptd real % location of terrain in D axis
 syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
 syms decl real; % earth magnetic field declination from true north
-syms R_YAW real; % variance for magnetic deviation measurement
+syms R_DECL R_YAW real; % variance of declination or yaw angle observation
-syms R_DECL real; % variance of supplied declination
 syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y  body axes
 syms rho real % air density (kg/m^3)
 syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
@@ -269,7 +268,28 @@ ccode(H_LOS,'file','H_LOS.txt');
 ccode(K_LOSX,'file','K_LOSX.txt');
 ccode(K_LOSY,'file','K_LOSY.txt');
 
-%% derive equations for fusion of direct yaw measurement
+%% derive equations for simple fusion of 2-D magnetic heading measurements
+
+% rotate magnetic field into earth axes
+magMeasNED = Tbn*[magX;magY;magZ];
+% the predicted measurement is the angle wrt true north of the horizontal
+% component of the measured field
+angMeas = atan(magMeasNED(2)/magMeasNED(1));
+simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
+Psimple = P(1:16,1:16);
+H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian
+%H_MAGS = H_MAGS(1:3);
+H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+%H_MAGS = simplify(H_MAGS);
+%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
+ccode(H_MAGS,'file','calcH_MAGS.c');
+% Calculate Kalman gain vector
+K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
+%K_MAGS = simplify(K_MAGS);
+%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
+ccode(K_MAGS,'file','calcK_MAGS.c');
+
+%% derive equations for fusion of yaw measurements
 
 % rotate X body axis into earth axes
 yawVec = Tbn*[1;0;0];
@@ -298,7 +318,7 @@ H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
 H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 H_MAGD = simplify(H_MAGD);
 %[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
-%ccode(H_MAGD,'file','calcH_MAGD.c');
+ccode(H_MAGD,'file','calcH_MAGD.c');
 % Calculate Kalman gain vector
 K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
 ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
@@ -339,11 +359,9 @@ H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
 H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 [H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
 K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
+ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
 [K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
 
-% generate c code for jacobians using the matlab code generator
-ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c');
-
 %% Save output and convert to m and c code fragments
 fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
 save(fileName);