diff --git a/EKF/common.h b/EKF/common.h
index 2e9b9f77ef..49ff1318ed 100644
--- a/EKF/common.h
+++ b/EKF/common.h
@@ -141,7 +141,7 @@ struct parameters {
 	float mag_declination_deg = 0.0f;   // magnetic declination in degrees
 	float heading_innov_gate = 3.0f;    // heading fusion innovation consistency gate size in standard deviations
 	float mag_innov_gate = 3.0f;        // magnetometer fusion innovation consistency gate size in standard deviations
-	int mag_declination_source = 7;     // bitmask used to control the handling of declination data
+	int mag_declination_source = 3;     // bitmask used to control the handling of declination data
 	int mag_fusion_type = 0;            // integer used to specify the type of magnetometer fusion used
 
 	// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
@@ -220,6 +220,7 @@ union filter_control_status_u {
 		uint8_t mag_dec     : 1; // 6 - true if synthetic magnetic declination measurements are being fused
 		uint8_t in_air      : 1; // 7 - true when the vehicle is airborne
 		uint8_t armed       : 1; // 8 - true when the vehicle motors are armed
+		uint8_t wind        : 1; // 9 - true when wind velocity is being estimated
 	} flags;
 	uint16_t value;
 };
diff --git a/EKF/control.cpp b/EKF/control.cpp
index bb33b11fcf..d2ceafb9a6 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -145,6 +145,14 @@ void Ekf::controlFusionModes()
 		_control_status.flags.mag_dec = false;
 	}
 
+	// Placeholder for control of wind velocity states estimation
+	// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement
+	if (false) {
+		_control_status.flags.wind = false;
+	}
+
+	// Store the status to enable change detection
+	_control_status_prev.value = _control_status.value;
 }
 
 void Ekf::calculateVehicleStatus()
diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 105a692602..bf360eef10 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -81,19 +81,22 @@ void Ekf::initialiseCovariance()
 	// accel z bias
 	P[15][15] = 0.0001f;
 
+	// variances for optional states
+	// these state variances are set to zero until the states are required, then they must be initialised
+
 	// earth magnetic field
-	P[16][16] = 0.0001f;
-	P[17][17] = 0.0001f;
-	P[18][18] = 0.0001f;
+	P[16][16] = 0.0f;
+	P[17][17] = 0.0f;
+	P[18][18] = 0.0f;
 
 	// body magnetic field
-	P[19][19] = 0.0001f;
-	P[20][20] = 0.0001f;
-	P[21][21] = 0.0001f;
+	P[19][19] = 0.0f;
+	P[20][20] = 0.0f;
+	P[21][21] = 0.0f;
 
 	// wind
-	P[22][22] = 0.01f;
-	P[23][23] = 0.01f;
+	P[22][22] = 0.0f;
+	P[23][23] = 0.0f;
 
 }
 
@@ -131,9 +134,33 @@ void Ekf::predictCovariance()
 	float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1e-4f);
 	float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1e-2f);
 	float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
-	float mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
-	float mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
-	float wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
+	float mag_I_sig, mag_B_sig;
+
+	// 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 (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) {
+		mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+
+	} else {
+		mag_I_sig = 0.0f;
+	}
+
+	// Don't continue to grow the earth 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
+	if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
+		mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
+
+	} else {
+		mag_B_sig = 0.0f;
+	}
+
+	float wind_vel_sig;
+
+	// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
+	if (_control_status.flags.wind && (P[22][22] + P[22][22]) < 1000.0f) {
+		wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
+
+	} else {
+		wind_vel_sig = 0.0f;
+	}
 
 	for (unsigned i = 0; i < 9; i++) {
 		process_noise[i] = 0.0f;
@@ -493,170 +520,200 @@ void Ekf::predictCovariance()
 	nextP[13][15] = P[13][15];
 	nextP[14][15] = P[14][15];
 	nextP[15][15] = P[15][15];
-	nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
-	nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
-	nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
-	nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
-	nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
-	nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
-	nextP[6][16] = P[6][16] + P[3][16] * dt;
-	nextP[7][16] = P[7][16] + P[4][16] * dt;
-	nextP[8][16] = P[8][16] + P[5][16] * dt;
-	nextP[9][16] = P[9][16];
-	nextP[10][16] = P[10][16];
-	nextP[11][16] = P[11][16];
-	nextP[12][16] = P[12][16];
-	nextP[13][16] = P[13][16];
-	nextP[14][16] = P[14][16];
-	nextP[15][16] = P[15][16];
-	nextP[16][16] = P[16][16];
-	nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
-	nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
-	nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
-	nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
-	nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
-	nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
-	nextP[6][17] = P[6][17] + P[3][17] * dt;
-	nextP[7][17] = P[7][17] + P[4][17] * dt;
-	nextP[8][17] = P[8][17] + P[5][17] * dt;
-	nextP[9][17] = P[9][17];
-	nextP[10][17] = P[10][17];
-	nextP[11][17] = P[11][17];
-	nextP[12][17] = P[12][17];
-	nextP[13][17] = P[13][17];
-	nextP[14][17] = P[14][17];
-	nextP[15][17] = P[15][17];
-	nextP[16][17] = P[16][17];
-	nextP[17][17] = P[17][17];
-	nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
-	nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
-	nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
-	nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
-	nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
-	nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
-	nextP[6][18] = P[6][18] + P[3][18] * dt;
-	nextP[7][18] = P[7][18] + P[4][18] * dt;
-	nextP[8][18] = P[8][18] + P[5][18] * dt;
-	nextP[9][18] = P[9][18];
-	nextP[10][18] = P[10][18];
-	nextP[11][18] = P[11][18];
-	nextP[12][18] = P[12][18];
-	nextP[13][18] = P[13][18];
-	nextP[14][18] = P[14][18];
-	nextP[15][18] = P[15][18];
-	nextP[16][18] = P[16][18];
-	nextP[17][18] = P[17][18];
-	nextP[18][18] = P[18][18];
-	nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
-	nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
-	nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
-	nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
-	nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
-	nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
-	nextP[6][19] = P[6][19] + P[3][19] * dt;
-	nextP[7][19] = P[7][19] + P[4][19] * dt;
-	nextP[8][19] = P[8][19] + P[5][19] * dt;
-	nextP[9][19] = P[9][19];
-	nextP[10][19] = P[10][19];
-	nextP[11][19] = P[11][19];
-	nextP[12][19] = P[12][19];
-	nextP[13][19] = P[13][19];
-	nextP[14][19] = P[14][19];
-	nextP[15][19] = P[15][19];
-	nextP[16][19] = P[16][19];
-	nextP[17][19] = P[17][19];
-	nextP[18][19] = P[18][19];
-	nextP[19][19] = P[19][19];
-	nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
-	nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
-	nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
-	nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
-	nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
-	nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
-	nextP[6][20] = P[6][20] + P[3][20] * dt;
-	nextP[7][20] = P[7][20] + P[4][20] * dt;
-	nextP[8][20] = P[8][20] + P[5][20] * dt;
-	nextP[9][20] = P[9][20];
-	nextP[10][20] = P[10][20];
-	nextP[11][20] = P[11][20];
-	nextP[12][20] = P[12][20];
-	nextP[13][20] = P[13][20];
-	nextP[14][20] = P[14][20];
-	nextP[15][20] = P[15][20];
-	nextP[16][20] = P[16][20];
-	nextP[17][20] = P[17][20];
-	nextP[18][20] = P[18][20];
-	nextP[19][20] = P[19][20];
-	nextP[20][20] = P[20][20];
-	nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
-	nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
-	nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
-	nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
-	nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
-	nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
-	nextP[6][21] = P[6][21] + P[3][21] * dt;
-	nextP[7][21] = P[7][21] + P[4][21] * dt;
-	nextP[8][21] = P[8][21] + P[5][21] * dt;
-	nextP[9][21] = P[9][21];
-	nextP[10][21] = P[10][21];
-	nextP[11][21] = P[11][21];
-	nextP[12][21] = P[12][21];
-	nextP[13][21] = P[13][21];
-	nextP[14][21] = P[14][21];
-	nextP[15][21] = P[15][21];
-	nextP[16][21] = P[16][21];
-	nextP[17][21] = P[17][21];
-	nextP[18][21] = P[18][21];
-	nextP[19][21] = P[19][21];
-	nextP[20][21] = P[20][21];
-	nextP[21][21] = P[21][21];
-	nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
-	nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
-	nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
-	nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
-	nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
-	nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
-	nextP[6][22] = P[6][22] + P[3][22] * dt;
-	nextP[7][22] = P[7][22] + P[4][22] * dt;
-	nextP[8][22] = P[8][22] + P[5][22] * dt;
-	nextP[9][22] = P[9][22];
-	nextP[10][22] = P[10][22];
-	nextP[11][22] = P[11][22];
-	nextP[12][22] = P[12][22];
-	nextP[13][22] = P[13][22];
-	nextP[14][22] = P[14][22];
-	nextP[15][22] = P[15][22];
-	nextP[16][22] = P[16][22];
-	nextP[17][22] = P[17][22];
-	nextP[18][22] = P[18][22];
-	nextP[19][22] = P[19][22];
-	nextP[20][22] = P[20][22];
-	nextP[21][22] = P[21][22];
-	nextP[22][22] = P[22][22];
-	nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
-	nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
-	nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
-	nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
-	nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
-	nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
-	nextP[6][23] = P[6][23] + P[3][23] * dt;
-	nextP[7][23] = P[7][23] + P[4][23] * dt;
-	nextP[8][23] = P[8][23] + P[5][23] * dt;
-	nextP[9][23] = P[9][23];
-	nextP[10][23] = P[10][23];
-	nextP[11][23] = P[11][23];
-	nextP[12][23] = P[12][23];
-	nextP[13][23] = P[13][23];
-	nextP[14][23] = P[14][23];
-	nextP[15][23] = P[15][23];
-	nextP[16][23] = P[16][23];
-	nextP[17][23] = P[17][23];
-	nextP[18][23] = P[18][23];
-	nextP[19][23] = P[19][23];
-	nextP[20][23] = P[20][23];
-	nextP[21][23] = P[21][23];
-	nextP[22][23] = P[22][23];
-	nextP[23][23] = P[23][23];
+
+	// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
+	if (_control_status.flags.mag_3D) {
+		// Check if we have just transitioned into 3-axis fusion and set the state variances
+		if (!_control_status_prev.flags.mag_3D) {
+			for (uint8_t index = 16; index <= 21; index++) {
+				P[index][index] = sq(fmaxf(_params.mag_noise, 0.001f));
+			}
+		}
+
+		nextP[0][16] = P[0][16] * SPP[5] - P[1][16] * SPP[4] + P[2][16] * SPP[7] + P[9][16] * SPP[22] + P[12][16] * SPP[18];
+		nextP[1][16] = P[1][16] * SPP[6] - P[0][16] * SPP[2] - P[2][16] * SPP[8] + P[10][16] * SPP[22] + P[13][16] * SPP[17];
+		nextP[2][16] = P[0][16] * SPP[14] - P[1][16] * SPP[3] + P[2][16] * SPP[13] + P[11][16] * SPP[22] + P[14][16] * SPP[16];
+		nextP[3][16] = P[3][16] + P[0][16] * SPP[1] + P[1][16] * SPP[19] + P[2][16] * SPP[15] - P[15][16] * SPP[21];
+		nextP[4][16] = P[4][16] + P[15][16] * SF[22] + P[0][16] * SPP[20] + P[1][16] * SPP[12] + P[2][16] * SPP[11];
+		nextP[5][16] = P[5][16] + P[15][16] * SF[20] - P[0][16] * SPP[9] + P[1][16] * SPP[10] + P[2][16] * SPP[0];
+		nextP[6][16] = P[6][16] + P[3][16] * dt;
+		nextP[7][16] = P[7][16] + P[4][16] * dt;
+		nextP[8][16] = P[8][16] + P[5][16] * dt;
+		nextP[9][16] = P[9][16];
+		nextP[10][16] = P[10][16];
+		nextP[11][16] = P[11][16];
+		nextP[12][16] = P[12][16];
+		nextP[13][16] = P[13][16];
+		nextP[14][16] = P[14][16];
+		nextP[15][16] = P[15][16];
+		nextP[16][16] = P[16][16];
+
+		nextP[0][17] = P[0][17] * SPP[5] - P[1][17] * SPP[4] + P[2][17] * SPP[7] + P[9][17] * SPP[22] + P[12][17] * SPP[18];
+		nextP[1][17] = P[1][17] * SPP[6] - P[0][17] * SPP[2] - P[2][17] * SPP[8] + P[10][17] * SPP[22] + P[13][17] * SPP[17];
+		nextP[2][17] = P[0][17] * SPP[14] - P[1][17] * SPP[3] + P[2][17] * SPP[13] + P[11][17] * SPP[22] + P[14][17] * SPP[16];
+		nextP[3][17] = P[3][17] + P[0][17] * SPP[1] + P[1][17] * SPP[19] + P[2][17] * SPP[15] - P[15][17] * SPP[21];
+		nextP[4][17] = P[4][17] + P[15][17] * SF[22] + P[0][17] * SPP[20] + P[1][17] * SPP[12] + P[2][17] * SPP[11];
+		nextP[5][17] = P[5][17] + P[15][17] * SF[20] - P[0][17] * SPP[9] + P[1][17] * SPP[10] + P[2][17] * SPP[0];
+		nextP[6][17] = P[6][17] + P[3][17] * dt;
+		nextP[7][17] = P[7][17] + P[4][17] * dt;
+		nextP[8][17] = P[8][17] + P[5][17] * dt;
+		nextP[9][17] = P[9][17];
+		nextP[10][17] = P[10][17];
+		nextP[11][17] = P[11][17];
+		nextP[12][17] = P[12][17];
+		nextP[13][17] = P[13][17];
+		nextP[14][17] = P[14][17];
+		nextP[15][17] = P[15][17];
+		nextP[16][17] = P[16][17];
+		nextP[17][17] = P[17][17];
+
+		nextP[0][18] = P[0][18] * SPP[5] - P[1][18] * SPP[4] + P[2][18] * SPP[7] + P[9][18] * SPP[22] + P[12][18] * SPP[18];
+		nextP[1][18] = P[1][18] * SPP[6] - P[0][18] * SPP[2] - P[2][18] * SPP[8] + P[10][18] * SPP[22] + P[13][18] * SPP[17];
+		nextP[2][18] = P[0][18] * SPP[14] - P[1][18] * SPP[3] + P[2][18] * SPP[13] + P[11][18] * SPP[22] + P[14][18] * SPP[16];
+		nextP[3][18] = P[3][18] + P[0][18] * SPP[1] + P[1][18] * SPP[19] + P[2][18] * SPP[15] - P[15][18] * SPP[21];
+		nextP[4][18] = P[4][18] + P[15][18] * SF[22] + P[0][18] * SPP[20] + P[1][18] * SPP[12] + P[2][18] * SPP[11];
+		nextP[5][18] = P[5][18] + P[15][18] * SF[20] - P[0][18] * SPP[9] + P[1][18] * SPP[10] + P[2][18] * SPP[0];
+		nextP[6][18] = P[6][18] + P[3][18] * dt;
+		nextP[7][18] = P[7][18] + P[4][18] * dt;
+		nextP[8][18] = P[8][18] + P[5][18] * dt;
+		nextP[9][18] = P[9][18];
+		nextP[10][18] = P[10][18];
+		nextP[11][18] = P[11][18];
+		nextP[12][18] = P[12][18];
+		nextP[13][18] = P[13][18];
+		nextP[14][18] = P[14][18];
+		nextP[15][18] = P[15][18];
+		nextP[16][18] = P[16][18];
+		nextP[17][18] = P[17][18];
+		nextP[18][18] = P[18][18];
+
+		nextP[0][19] = P[0][19] * SPP[5] - P[1][19] * SPP[4] + P[2][19] * SPP[7] + P[9][19] * SPP[22] + P[12][19] * SPP[18];
+		nextP[1][19] = P[1][19] * SPP[6] - P[0][19] * SPP[2] - P[2][19] * SPP[8] + P[10][19] * SPP[22] + P[13][19] * SPP[17];
+		nextP[2][19] = P[0][19] * SPP[14] - P[1][19] * SPP[3] + P[2][19] * SPP[13] + P[11][19] * SPP[22] + P[14][19] * SPP[16];
+		nextP[3][19] = P[3][19] + P[0][19] * SPP[1] + P[1][19] * SPP[19] + P[2][19] * SPP[15] - P[15][19] * SPP[21];
+		nextP[4][19] = P[4][19] + P[15][19] * SF[22] + P[0][19] * SPP[20] + P[1][19] * SPP[12] + P[2][19] * SPP[11];
+		nextP[5][19] = P[5][19] + P[15][19] * SF[20] - P[0][19] * SPP[9] + P[1][19] * SPP[10] + P[2][19] * SPP[0];
+		nextP[6][19] = P[6][19] + P[3][19] * dt;
+		nextP[7][19] = P[7][19] + P[4][19] * dt;
+		nextP[8][19] = P[8][19] + P[5][19] * dt;
+		nextP[9][19] = P[9][19];
+		nextP[10][19] = P[10][19];
+		nextP[11][19] = P[11][19];
+		nextP[12][19] = P[12][19];
+		nextP[13][19] = P[13][19];
+		nextP[14][19] = P[14][19];
+		nextP[15][19] = P[15][19];
+		nextP[16][19] = P[16][19];
+		nextP[17][19] = P[17][19];
+		nextP[18][19] = P[18][19];
+		nextP[19][19] = P[19][19];
+
+		nextP[0][20] = P[0][20] * SPP[5] - P[1][20] * SPP[4] + P[2][20] * SPP[7] + P[9][20] * SPP[22] + P[12][20] * SPP[18];
+		nextP[1][20] = P[1][20] * SPP[6] - P[0][20] * SPP[2] - P[2][20] * SPP[8] + P[10][20] * SPP[22] + P[13][20] * SPP[17];
+		nextP[2][20] = P[0][20] * SPP[14] - P[1][20] * SPP[3] + P[2][20] * SPP[13] + P[11][20] * SPP[22] + P[14][20] * SPP[16];
+		nextP[3][20] = P[3][20] + P[0][20] * SPP[1] + P[1][20] * SPP[19] + P[2][20] * SPP[15] - P[15][20] * SPP[21];
+		nextP[4][20] = P[4][20] + P[15][20] * SF[22] + P[0][20] * SPP[20] + P[1][20] * SPP[12] + P[2][20] * SPP[11];
+		nextP[5][20] = P[5][20] + P[15][20] * SF[20] - P[0][20] * SPP[9] + P[1][20] * SPP[10] + P[2][20] * SPP[0];
+		nextP[6][20] = P[6][20] + P[3][20] * dt;
+		nextP[7][20] = P[7][20] + P[4][20] * dt;
+		nextP[8][20] = P[8][20] + P[5][20] * dt;
+		nextP[9][20] = P[9][20];
+		nextP[10][20] = P[10][20];
+		nextP[11][20] = P[11][20];
+		nextP[12][20] = P[12][20];
+		nextP[13][20] = P[13][20];
+		nextP[14][20] = P[14][20];
+		nextP[15][20] = P[15][20];
+		nextP[16][20] = P[16][20];
+		nextP[17][20] = P[17][20];
+		nextP[18][20] = P[18][20];
+		nextP[19][20] = P[19][20];
+		nextP[20][20] = P[20][20];
+
+		nextP[0][21] = P[0][21] * SPP[5] - P[1][21] * SPP[4] + P[2][21] * SPP[7] + P[9][21] * SPP[22] + P[12][21] * SPP[18];
+		nextP[1][21] = P[1][21] * SPP[6] - P[0][21] * SPP[2] - P[2][21] * SPP[8] + P[10][21] * SPP[22] + P[13][21] * SPP[17];
+		nextP[2][21] = P[0][21] * SPP[14] - P[1][21] * SPP[3] + P[2][21] * SPP[13] + P[11][21] * SPP[22] + P[14][21] * SPP[16];
+		nextP[3][21] = P[3][21] + P[0][21] * SPP[1] + P[1][21] * SPP[19] + P[2][21] * SPP[15] - P[15][21] * SPP[21];
+		nextP[4][21] = P[4][21] + P[15][21] * SF[22] + P[0][21] * SPP[20] + P[1][21] * SPP[12] + P[2][21] * SPP[11];
+		nextP[5][21] = P[5][21] + P[15][21] * SF[20] - P[0][21] * SPP[9] + P[1][21] * SPP[10] + P[2][21] * SPP[0];
+		nextP[6][21] = P[6][21] + P[3][21] * dt;
+		nextP[7][21] = P[7][21] + P[4][21] * dt;
+		nextP[8][21] = P[8][21] + P[5][21] * dt;
+		nextP[9][21] = P[9][21];
+		nextP[10][21] = P[10][21];
+		nextP[11][21] = P[11][21];
+		nextP[12][21] = P[12][21];
+		nextP[13][21] = P[13][21];
+		nextP[14][21] = P[14][21];
+		nextP[15][21] = P[15][21];
+		nextP[16][21] = P[16][21];
+		nextP[17][21] = P[17][21];
+		nextP[18][21] = P[18][21];
+		nextP[19][21] = P[19][21];
+		nextP[20][21] = P[20][21];
+		nextP[21][21] = P[21][21];
+	}
+
+	// Don't do covariance prediction on wind states unless we are using them
+	if (_control_status.flags.wind) {
+		// Check if we have jsut transitioned to using wind states and set the variances accordingly
+		if (!_control_status_prev.flags.mag_3D) {
+			for (uint8_t index = 22; index <= 23; index++) {
+				// TODO initialise wind states using ground speed and airspeed and set initial variance using sum of ground speed and airspeed variances
+				P[index][index] = sq(5.0f);
+			}
+		}
+
+		nextP[0][22] = P[0][22] * SPP[5] - P[1][22] * SPP[4] + P[2][22] * SPP[7] + P[9][22] * SPP[22] + P[12][22] * SPP[18];
+		nextP[1][22] = P[1][22] * SPP[6] - P[0][22] * SPP[2] - P[2][22] * SPP[8] + P[10][22] * SPP[22] + P[13][22] * SPP[17];
+		nextP[2][22] = P[0][22] * SPP[14] - P[1][22] * SPP[3] + P[2][22] * SPP[13] + P[11][22] * SPP[22] + P[14][22] * SPP[16];
+		nextP[3][22] = P[3][22] + P[0][22] * SPP[1] + P[1][22] * SPP[19] + P[2][22] * SPP[15] - P[15][22] * SPP[21];
+		nextP[4][22] = P[4][22] + P[15][22] * SF[22] + P[0][22] * SPP[20] + P[1][22] * SPP[12] + P[2][22] * SPP[11];
+		nextP[5][22] = P[5][22] + P[15][22] * SF[20] - P[0][22] * SPP[9] + P[1][22] * SPP[10] + P[2][22] * SPP[0];
+		nextP[6][22] = P[6][22] + P[3][22] * dt;
+		nextP[7][22] = P[7][22] + P[4][22] * dt;
+		nextP[8][22] = P[8][22] + P[5][22] * dt;
+		nextP[9][22] = P[9][22];
+		nextP[10][22] = P[10][22];
+		nextP[11][22] = P[11][22];
+		nextP[12][22] = P[12][22];
+		nextP[13][22] = P[13][22];
+		nextP[14][22] = P[14][22];
+		nextP[15][22] = P[15][22];
+		nextP[16][22] = P[16][22];
+		nextP[17][22] = P[17][22];
+		nextP[18][22] = P[18][22];
+		nextP[19][22] = P[19][22];
+		nextP[20][22] = P[20][22];
+		nextP[21][22] = P[21][22];
+		nextP[22][22] = P[22][22];
+
+		nextP[0][23] = P[0][23] * SPP[5] - P[1][23] * SPP[4] + P[2][23] * SPP[7] + P[9][23] * SPP[22] + P[12][23] * SPP[18];
+		nextP[1][23] = P[1][23] * SPP[6] - P[0][23] * SPP[2] - P[2][23] * SPP[8] + P[10][23] * SPP[22] + P[13][23] * SPP[17];
+		nextP[2][23] = P[0][23] * SPP[14] - P[1][23] * SPP[3] + P[2][23] * SPP[13] + P[11][23] * SPP[22] + P[14][23] * SPP[16];
+		nextP[3][23] = P[3][23] + P[0][23] * SPP[1] + P[1][23] * SPP[19] + P[2][23] * SPP[15] - P[15][23] * SPP[21];
+		nextP[4][23] = P[4][23] + P[15][23] * SF[22] + P[0][23] * SPP[20] + P[1][23] * SPP[12] + P[2][23] * SPP[11];
+		nextP[5][23] = P[5][23] + P[15][23] * SF[20] - P[0][23] * SPP[9] + P[1][23] * SPP[10] + P[2][23] * SPP[0];
+		nextP[6][23] = P[6][23] + P[3][23] * dt;
+		nextP[7][23] = P[7][23] + P[4][23] * dt;
+		nextP[8][23] = P[8][23] + P[5][23] * dt;
+		nextP[9][23] = P[9][23];
+		nextP[10][23] = P[10][23];
+		nextP[11][23] = P[11][23];
+		nextP[12][23] = P[12][23];
+		nextP[13][23] = P[13][23];
+		nextP[14][23] = P[14][23];
+		nextP[15][23] = P[15][23];
+		nextP[16][23] = P[16][23];
+		nextP[17][23] = P[17][23];
+		nextP[18][23] = P[18][23];
+		nextP[19][23] = P[19][23];
+		nextP[20][23] = P[20][23];
+		nextP[21][23] = P[21][23];
+		nextP[22][23] = P[22][23];
+		nextP[23][23] = P[23][23];
+
+	}
 
 	// add process noise
 	for (unsigned i = 0; i < _k_num_states; i++) {
@@ -664,7 +721,7 @@ void Ekf::predictCovariance()
 	}
 
 	// stop position covariance growth if our total position variance reaches 100m
-	// this can happen if we loose gps for some time
+	// this can happen if we lose gps for some time
 	if ((P[6][6] + P[7][7]) > 1e4f) {
 		for (uint8_t i = 6; i < 8; i++) {
 			for (uint8_t j = 0; j < _k_num_states; j++) {
@@ -681,10 +738,12 @@ void Ekf::predictCovariance()
 		}
 	}
 
+	// copy variances (diagonals)
 	for (unsigned i = 0; i < _k_num_states; i++) {
 		P[i][i] = nextP[i][i];
 	}
 
+	// force symmetry
 	for (unsigned row = 1; row < _k_num_states; row++) {
 		for (unsigned column = 0; column < row; column++) {
 			P[row][column] = 0.5f * (nextP[row][column] + nextP[column][row]);
@@ -706,8 +765,8 @@ void Ekf::limitCov()
 	P_lim[3] = 0.001f;		// gyro bias max var
 	P_lim[4] = 0.01f;		// gyro scale max var
 	P_lim[5] = 0.1f;		// delta velocity z bias max var
-	P_lim[6] = 0.01f;		// earth mag field max var
-	P_lim[7] = 0.01f;		// body mag field max var
+	P_lim[6] = 0.1f;		// earth mag field max var
+	P_lim[7] = 0.1f;		// body mag field max var
 	P_lim[8] = 1000.0f;		// wind max var
 
 	for (int i = 0; i < 3; i++) {
@@ -742,20 +801,14 @@ void Ekf::limitCov()
 	math::constrain(P[15][15], 0.0f, P_lim[5]);
 
 	for (int i = 16; i < 19; i++) {
-
 		math::constrain(P[i][i], 0.0f, P_lim[6]);
 	}
 
 	for (int i = 19; i < 22; i++) {
-
-
 		math::constrain(P[i][i], 0.0f, P_lim[7]);
 	}
 
 	for (int i = 22; i < 24; i++) {
-
 		math::constrain(P[i][i], 0.0f, P_lim[8]);
 	}
 }
-
-
diff --git a/EKF/ekf.h b/EKF/ekf.h
index de693135e9..a7a426463a 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -85,8 +85,12 @@ public:
 	bool collect_gps(uint64_t time_usec, struct gps_message *gps);
 	bool collect_imu(imuSample &imu);
 
+	// this is the current status of the filter control modes
 	filter_control_status_u _control_status = {};
 
+	// this is the previous status of the filter control modes - used to detect mode transitions
+	filter_control_status_u _control_status_prev = {};
+
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
 
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index e1f7857063..9434266921 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -284,10 +284,18 @@ void Ekf::fuseMag()
 						  SK_MX[1] + P[20][18] * SK_MX[3]);
 			Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] *
 						  SK_MX[1] + P[21][18] * SK_MX[3]);
-			Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
-						  SK_MX[1] + P[22][18] * SK_MX[3]);
-			Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
-						  SK_MX[1] + P[23][18] * SK_MX[3]);
+
+			// Don't update wind states unless we are doing wind estimation
+			if (_control_status.flags.wind) {
+				Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
+							  SK_MX[1] + P[22][18] * SK_MX[3]);
+				Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
+							  SK_MX[1] + P[23][18] * SK_MX[3]);
+
+			} else {
+				Kfusion[22] = 0.0f;
+				Kfusion[23] = 0.0f;
+			}
 
 		} else if (index == 1) {
 			// Calculate Y axis Kalman gains
@@ -340,10 +348,18 @@ void Ekf::fuseMag()
 						  SK_MY[1] - P[20][16] * SK_MY[3]);
 			Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] *
 						  SK_MY[1] - P[21][16] * SK_MY[3]);
-			Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
-						  SK_MY[1] - P[22][16] * SK_MY[3]);
-			Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
-						  SK_MY[1] - P[23][16] * SK_MY[3]);
+
+			// Don't update wind states unless we are doing wind estimation
+			if (_control_status.flags.wind) {
+				Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
+							  SK_MY[1] - P[22][16] * SK_MY[3]);
+				Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
+							  SK_MY[1] - P[23][16] * SK_MY[3]);
+
+			} else {
+				Kfusion[22] = 0.0f;
+				Kfusion[23] = 0.0f;
+			}
 
 		} else if (index == 2) {
 			// Calculate Z axis Kalman gains
@@ -396,10 +412,18 @@ void Ekf::fuseMag()
 						  SK_MZ[2] - P[20][17] * SK_MZ[3]);
 			Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] *
 						  SK_MZ[2] - P[21][17] * SK_MZ[3]);
-			Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
-						  SK_MZ[2] - P[22][17] * SK_MZ[3]);
-			Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
-						  SK_MZ[2] - P[23][17] * SK_MZ[3]);
+
+			// Don't update wind states unless we are doing wind estimation
+			if (_control_status.flags.wind) {
+				Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
+							  SK_MZ[2] - P[22][17] * SK_MZ[3]);
+				Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
+							  SK_MZ[2] - P[23][17] * SK_MZ[3]);
+
+			} else {
+				Kfusion[22] = 0.0f;
+				Kfusion[23] = 0.0f;
+			}
 
 		} else {
 			return;
@@ -468,73 +492,71 @@ void Ekf::fuseHeading()
 	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_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f);
-	R_mag = R_mag * R_mag;
+	float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f);
+	R_YAW = R_YAW * R_YAW;
 
+	// 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 * q2 * 2.0f;
-	float t7 = q1 * q3 * 2.0f;
-	float t8 = t6 + t7;
-	float t9 = q0 * q3 * 2.0f;
-	float t13 = q1 * q2 * 2.0f;
-	float t10 = t9 - t13;
-	float t11 = t2 + t3 - t4 - t5;
-	float t12 = magX * t11;
-	float t14 = magZ * t8;
-	float t19 = magY * t10;
-	float t15 = t12 + t14 - t19;
-	float t16 = t2 - t3 + t4 - t5;
-	float t17 = q0 * q1 * 2.0f;
-	float t24 = q2 * q3 * 2.0f;
-	float t18 = t17 - t24;
-	float t20 = 1.0f / t15;
-	float t21 = magY * t16;
-	float t22 = t9 + t13;
-	float t23 = magX * t22;
-	float t28 = magZ * t18;
-	float t25 = t21 + t23 - t28;
-	float t29 = t20 * t25;
-	float t26 = tan(t29);
-	float t27 = 1.0f / (t15 * t15);
-	float t30 = t26 * t26;
-	float t31 = t30 + 1.0f;
+	float t6 = t2 + t3 - t4 - t5;
+	float t7 = q0 * q3 * 2.0f;
+	float t8 = q1 * q2 * 2.0f;
+	float t9 = t7 + t8;
+	float t10;
 
-	float H_HDG[3] = {};
-	H_HDG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10));
-	H_HDG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11));
-	H_HDG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11));
+	if (fabsf(t6) > 1e-6f) {
+		t10 = 1.0f / (t6 * t6);
+
+	} else  {
+		return;
+	}
+
+	float t11 = t9 * t9;
+	float t12 = t10 * t11;
+	float t13 = t12 + 1.0f;
+	float t14;
+
+	if (fabsf(t13) > 1e-6f) {
+		t14 = 1.0f / t13;
+
+	} else  {
+		return;
+	}
+
+	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));
 
 	// calculate innovation
+	// rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement
+	// TODO - enable use of an off-board heading measurement
 	matrix::Dcm<float> R_to_earth(_state.quat_nominal);
 	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
-
 	float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
 
-	innovation = math::constrain(innovation, -0.5f, 0.5f);
+	// wrap the innovation to the interval between +-pi
+	innovation = matrix::wrap_pi(innovation);
 	_heading_innov = innovation;
 
-	float innovation_var = R_mag;
-	_heading_innov_var = innovation_var;
-
 	// 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_HDG[column];
+			PH[row] += P[row][column] * H_YAW[column];
 		}
 
-		innovation_var += H_HDG[row] * PH[row];
+		innovation_var += H_YAW[row] * PH[row];
 	}
 
-	if (innovation_var >= R_mag) {
+	if (innovation_var >= R_YAW) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.bad_mag_hdg = false;
 
@@ -547,19 +569,42 @@ void Ekf::fuseHeading()
 		return;
 	}
 
-	float innovation_var_inv = 1 / innovation_var;
+	float innovation_var_inv = 1.0f / innovation_var;
 
-	// calculate kalman gain
+	// calculate the kalman gains taking dvantage of the reduce size of H_YAW
 	float Kfusion[_k_num_states] = {};
 
-	for (unsigned row = 0; row < _k_num_states; row++) {
+	// gains for states that are always used
+	for (unsigned row = 0; row <= 15; row++) {
 		for (unsigned column = 0; column < 3; column++) {
-			Kfusion[row] += P[row][column] * H_HDG[column];
+			Kfusion[row] += P[row][column] * H_YAW[column];
 		}
 
 		Kfusion[row] *= innovation_var_inv;
 	}
 
+	// 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++) {
+			for (unsigned column = 0; column < 3; column++) {
+				Kfusion[row] += P[row][column] * H_YAW[column];
+			}
+
+			Kfusion[row] *= innovation_var_inv;
+		}
+	}
+
 	// innovation test ratio
 	_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
 
@@ -572,12 +617,18 @@ void Ekf::fuseHeading()
 		// by interference or a large initial gyro bias
 		if (_control_status.flags.in_air) {
 			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)) * innovation_var));
+			innovation = math::constrain(innovation, -gate_limit, gate_limit);
 		}
 
 	} else {
 		_mag_healthy = true;
 	}
 
+	// zero the attitude error states and use the kalman gain vector and innovation to update the states
 	_state.ang_error.setZero();
 	fuse(Kfusion, innovation);
 
@@ -587,11 +638,12 @@ void Ekf::fuseHeading()
 	_state.quat_nominal = dq * _state.quat_nominal;
 	_state.quat_nominal.normalize();
 
+	// update the covariance matrix taking advantage of the reduced size of H_YAW
 	float HP[_k_num_states] = {};
 
 	for (unsigned column = 0; column < _k_num_states; column++) {
 		for (unsigned row = 0; row < 3; row++) {
-			HP[column] += H_HDG[row] * P[row][column];
+			HP[column] += H_YAW[row] * P[row][column];
 		}
 	}
 
@@ -615,30 +667,28 @@ void Ekf::fuseDeclination()
 
 	// Calculate intermediate variables
 	// if the horizontal magnetic field is too small, this calculation will be badly conditioned
-	if (fabsf(magN) < 0.001f) {
+	if (magN < 0.001f) {
 		return;
 	}
 
-	float t2 = 1.0f / magN;
-	float t4 = magE * t2;
-	float t3 = tanf(t4);
-	float t5 = t3 * t3;
-	float t6 = t5 + 1.0f;
-	float t25 = t2 * t6;
-	float t7 = 1.0f / (magN * magN);
-	float t26 = magE * t6 * t7;
-	float t8 = P[17][17] * t25;
-	float t15 = P[16][17] * t26;
-	float t9 = t8 - t15;
-	float t10 = t25 * t9;
-	float t11 = P[17][16] * t25;
-	float t16 = P[16][16] * t26;
-	float t12 = t11 - t16;
-	float t17 = t26 * t12;
-	float t13 = R_DECL + t10 - t17; // innovation variance
+	float t2 = magE * magE;
+	float t3 = magN * magN;
+	float t4 = t2 + t3;
+	float t5 = 1.0f / t4;
+	float t22 = magE * t5;
+	float t23 = magN * t5;
+	float t6 = P[16][16] * t22;
+	float t13 = P[17][16] * t23;
+	float t7 = t6 - t13;
+	float t8 = t22 * t7;
+	float t9 = P[16][17] * t22;
+	float t14 = P[17][17] * t23;
+	float t10 = t9 - t14;
+	float t15 = t23 * t10;
+	float t11 = R_DECL + t8 - t15; // innovation variance
 
 	// check the innovation variance calculation for a badly conditioned covariance matrix
-	if (t13 >= R_DECL) {
+	if (t11 >= R_DECL) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.bad_mag_decl = false;
 
@@ -651,50 +701,53 @@ void Ekf::fuseDeclination()
 		return;
 	}
 
-	float t14 = 1.0f / t13;
-	float t18 = magE;
-	float t19 = magN;
-	float t21 = 1.0f / t19;
-	float t22 = t18 * t21;
-	float t20 = tanf(t22);
-	float t23 = t20 * t20;
-	float t24 = t23 + 1.0f;
+	float t12 = 1.0f / t11;
 
 	// Calculate the observation Jacobian
 	// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
 	float H_DECL[24] = {};
-	H_DECL[16] = -t18 * 1.0f / (t19 * t19) * t24;
-	H_DECL[17] = t21 * t24;
+	H_DECL[16] = -magE * t5;
+	H_DECL[17] = magN * t5;
 
 	// Calculate the Kalman gains
 	float Kfusion[_k_num_states] = {};
-	Kfusion[0] = t14 * (P[0][17] * t25 - P[0][16] * t26);
-	Kfusion[1] = t14 * (P[1][17] * t25 - P[1][16] * t26);
-	Kfusion[2] = t14 * (P[2][17] * t25 - P[2][16] * t26);
-	Kfusion[3] = t14 * (P[3][17] * t25 - P[3][16] * t26);
-	Kfusion[4] = t14 * (P[4][17] * t25 - P[4][16] * t26);
-	Kfusion[5] = t14 * (P[5][17] * t25 - P[5][16] * t26);
-	Kfusion[6] = t14 * (P[6][17] * t25 - P[6][16] * t26);
-	Kfusion[7] = t14 * (P[7][17] * t25 - P[7][16] * t26);
-	Kfusion[8] = t14 * (P[8][17] * t25 - P[8][16] * t26);
-	Kfusion[9] = t14 * (P[9][17] * t25 - P[9][16] * t26);
-	Kfusion[10] = t14 * (P[10][17] * t25 - P[10][16] * t26);
-	Kfusion[11] = t14 * (P[11][17] * t25 - P[11][16] * t26);
-	Kfusion[12] = t14 * (P[12][17] * t25 - P[12][16] * t26);
-	Kfusion[13] = t14 * (P[13][17] * t25 - P[13][16] * t26);
-	Kfusion[14] = t14 * (P[14][17] * t25 - P[14][16] * t26);
-	Kfusion[15] = t14 * (P[15][17] * t25 - P[15][16] * t26);
-	Kfusion[16] = -t14 * (t16 - P[16][17] * t25);
-	Kfusion[17] = t14 * (t8 - P[17][16] * t26);
-	Kfusion[18] = t14 * (P[18][17] * t25 - P[18][16] * t26);
-	Kfusion[19] = t14 * (P[19][17] * t25 - P[19][16] * t26);
-	Kfusion[20] = t14 * (P[20][17] * t25 - P[20][16] * t26);
-	Kfusion[21] = t14 * (P[21][17] * t25 - P[21][16] * t26);
-	Kfusion[22] = t14 * (P[22][17] * t25 - P[22][16] * t26);
-	Kfusion[23] = t14 * (P[23][17] * t25 - P[23][16] * t26);
+	Kfusion[0] = -t12 * (P[0][16] * t22 - P[0][17] * t23);
+	Kfusion[1] = -t12 * (P[1][16] * t22 - P[1][17] * t23);
+	Kfusion[2] = -t12 * (P[2][16] * t22 - P[2][17] * t23);
+	Kfusion[3] = -t12 * (P[3][16] * t22 - P[3][17] * t23);
+	Kfusion[4] = -t12 * (P[4][16] * t22 - P[4][17] * t23);
+	Kfusion[5] = -t12 * (P[5][16] * t22 - P[5][17] * t23);
+	Kfusion[6] = -t12 * (P[6][16] * t22 - P[6][17] * t23);
+	Kfusion[7] = -t12 * (P[7][16] * t22 - P[7][17] * t23);
+	Kfusion[8] = -t12 * (P[8][16] * t22 - P[8][17] * t23);
+	Kfusion[9] = -t12 * (P[9][16] * t22 - P[9][17] * t23);
+	Kfusion[10] = -t12 * (P[10][16] * t22 - P[10][17] * t23);
+	Kfusion[11] = -t12 * (P[11][16] * t22 - P[11][17] * t23);
+	Kfusion[12] = -t12 * (P[12][16] * t22 - P[12][17] * t23);
+	Kfusion[13] = -t12 * (P[13][16] * t22 - P[13][17] * t23);
+	Kfusion[14] = -t12 * (P[14][16] * t22 - P[14][17] * t23);
+	Kfusion[15] = -t12 * (P[15][16] * t22 - P[15][17] * t23);
+
+	// We only do declination fusion when we are using all the field states, so no logic required here
+	Kfusion[16] = -t12 * (t6 - P[16][17] * t23);
+	Kfusion[17] = t12 * (t14 - P[17][16] * t22);
+	Kfusion[18] = -t12 * (P[18][16] * t22 - P[18][17] * t23);
+	Kfusion[19] = -t12 * (P[19][16] * t22 - P[19][17] * t23);
+	Kfusion[20] = -t12 * (P[20][16] * t22 - P[20][17] * t23);
+	Kfusion[21] = -t12 * (P[21][16] * t22 - P[21][17] * t23);
+
+	// Don't update wind states unless we are doing wind estimation
+	if (_control_status.flags.wind) {
+		Kfusion[22] = -t12 * (P[22][16] * t22 - P[22][17] * t23);
+		Kfusion[23] = -t12 * (P[23][16] * t22 - P[23][17] * t23);
+
+	} else {
+		Kfusion[22] = 0.0f;
+		Kfusion[23] = 0.0f;
+	}
 
 	// calculate innovation and constrain
-	float innovation = atanf(t4) - _mag_declination;
+	float innovation = atanf(magE / magN) - _mag_declination;
 	innovation = math::constrain(innovation, -0.5f, 0.5f);
 
 	// zero attitude error states and perform the state correction
diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp
index dcc695262f..6ec7300a89 100644
--- a/EKF/vel_pos_fusion.cpp
+++ b/EKF/vel_pos_fusion.cpp
@@ -153,9 +153,29 @@ void Ekf::fuseVelPosHeight()
 		unsigned state_index = obs_index + 3;	// we start with vx and this is the 4. state
 
         // calculate kalman gain K = PHS, where S = 1/innovation variance
-		for (int row = 0; row < 24; row++) {
-            Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+                for (int row = 0; row <= 15; row++) {
+                    Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
 		}
+                // only update magnetic field states if we are fusing 3-axis observations
+                if (_control_status.flags.mag_3D) {
+                    for (int row = 16; row <= 21; row++) {
+                        Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+                    }
+                } else {
+                    for (int row = 16; row <= 21; row++) {
+                        Kfusion[row] = 0.0f;
+                    }
+                }
+                // only update wind states if we are doing wind estimation
+                if (_control_status.flags.wind) {
+                    for (int row = 22; row <= 23; row++) {
+                        Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
+                    }
+                } else {
+                    for (int row = 22; row <= 23; row++) {
+                        Kfusion[row] = 0.0f;
+                    }
+                }
 
 		// by definition the angle error state is zero at the fusion time
 		_state.ang_error.setZero();
diff --git a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt
index 8e1d146575..c1455008a5 100644
--- a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt	
+++ b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt	
@@ -9,62 +9,56 @@ declinationAngle is the estimated declination as that location
 This fusion method is used to constrain the rotation of the earth field vector when there are no earth relative measurements 
 (e.g. using optical flow without GPS, or when the vehicle is stationary) to provide an absolute yaw reference. In this situation the presence of yaw gyro errors
 can cause the magnetic declination of the earth field estimates to slowly rotate.
+
+Divide by zero protection and protection against a badly conditioned covariance matrix must be included.
 */
 
 // Calculate intermediate variable
-float t2 = 1.0f/magN;
-float t4 = magE*t2;
-float t3 = tanf(t4);
-float t5 = t3*t3;
-float t6 = t5+1.0f;
-float t25 = t2*t6;
-float t7 = 1.0f/(magN*magN);
-float t26 = magE*t6*t7;
-float t8 = P[17][17]*t25;
-float t15 = P[16][17]*t26;
-float t9 = t8-t15;
-float t10 = t25*t9;
-float t11 = P[17][16]*t25;
-float t16 = P[16][16]*t26;
-float t12 = t11-t16;
-float t17 = t26*t12;
-float t13 = R_DECL+t10-t17;
-float t14 = 1.0f/t13;
-float t18 = magE;
-float t19 = magN;
-float t21 = 1.0f/t19;
-float t22 = t18*t21;
-float t20 = tanf(t22);
-float t23 = t20*t20;
-float t24 = t23+1.0f;
+float t2 = magE*magE;
+float t3 = magN*magN;
+float t4 = t2+t3;
+float t5 = 1.0f/t4;
+float t22 = magE*t5;
+float t23 = magN*t5;
+float t6 = P[16][16]*t22;
+float t13 = P[17][16]*t23;
+float t7 = t6-t13;
+float t8 = t22*t7;
+float t9 = P[16][17]*t22;
+float t14 = P[17][17]*t23;
+float t10 = t9-t14;
+float t15 = t23*t10;
+float t11 = R_DECL+t8-t15; // innovation variance
+float t12 = 1.0f/t11;
 
 // Calculate the observation Jacobian
 // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
-H_MAG[16] = -t18*1.0f/(t19*t19)*t24;
-H_MAG[17] = t21*t24;
+H_DECL[16] = -magE*t5;
+H_DECL[17] = magN*t5;
 
 // Calculate the Kalman gains
-Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26);
-Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26);
-Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26);
-Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26);
-Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26);
-Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26);
-Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26);
-Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26);
-Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26);
-Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26);
-Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26);
-Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26);
-Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26);
-Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26);
-Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26);
-Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26);
-Kfusion[16] = -t14*(t16-P[16][17]*t25);
-Kfusion[17] = t14*(t8-P[17][16]*t26);
-Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26);
-Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26);
-Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26);
-Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26);
-Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26);
-Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26);
+Kfusion[0] = -t12*(P[0][16]*t22-P[0][17]*t23);
+Kfusion[1] = -t12*(P[1][16]*t22-P[1][17]*t23);
+Kfusion[2] = -t12*(P[2][16]*t22-P[2][17]*t23);
+Kfusion[3] = -t12*(P[3][16]*t22-P[3][17]*t23);
+Kfusion[4] = -t12*(P[4][16]*t22-P[4][17]*t23);
+Kfusion[5] = -t12*(P[5][16]*t22-P[5][17]*t23);
+Kfusion[6] = -t12*(P[6][16]*t22-P[6][17]*t23);
+Kfusion[7] = -t12*(P[7][16]*t22-P[7][17]*t23);
+Kfusion[8] = -t12*(P[8][16]*t22-P[8][17]*t23);
+Kfusion[9] = -t12*(P[9][16]*t22-P[9][17]*t23);
+Kfusion[10] = -t12*(P[10][16]*t22-P[10][17]*t23);
+Kfusion[11] = -t12*(P[11][16]*t22-P[11][17]*t23);
+Kfusion[12] = -t12*(P[12][16]*t22-P[12][17]*t23);
+Kfusion[13] = -t12*(P[13][16]*t22-P[13][17]*t23);
+Kfusion[14] = -t12*(P[14][16]*t22-P[14][17]*t23);
+Kfusion[15] = -t12*(P[15][16]*t22-P[15][17]*t23);
+Kfusion[16] = -t12*(t6-P[16][17]*t23);
+Kfusion[17] = t12*(t14-P[17][16]*t22);
+Kfusion[18] = -t12*(P[18][16]*t22-P[18][17]*t23);
+Kfusion[19] = -t12*(P[19][16]*t22-P[19][17]*t23);
+Kfusion[20] = -t12*(P[20][16]*t22-P[20][17]*t23);
+Kfusion[21] = -t12*(P[21][16]*t22-P[21][17]*t23);
+Kfusion[22] = -t12*(P[22][16]*t22-P[22][17]*t23);
+Kfusion[23] = -t12*(P[23][16]*t22-P[23][17]*t23);
+
diff --git a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt
index 606c51d51b..ea4a5ba1d3 100644
--- a/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt	
+++ b/matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt	
@@ -5,6 +5,8 @@ innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationA
 
 magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
 declinationAngle is the estimated declination as that location
+
+Protection against /0 and covariance matrix errors will need to be added.
 */
 
 // intermediate variables
@@ -12,36 +14,19 @@ float t2 = q0*q0;
 float t3 = q1*q1;
 float t4 = q2*q2;
 float t5 = q3*q3;
-float t6 = q0*q2*2.0f;
-float t7 = q1*q3*2.0f;
-float t8 = t6+t7;
-float t9 = q0*q3*2.0f;
-float t13 = q1*q2*2.0f;
-float t10 = t9-t13;
-float t11 = t2+t3-t4-t5;
-float t12 = magX*t11;
-float t14 = magZ*t8;
-float t19 = magY*t10;
-float t15 = t12+t14-t19;
-float t16 = t2-t3+t4-t5;
-float t17 = q0*q1*2.0f;
-float t24 = q2*q3*2.0f;
-float t18 = t17-t24;
-float t20 = 1.0f/t15;
-float t21 = magY*t16;
-float t22 = t9+t13;
-float t23 = magX*t22;
-float t28 = magZ*t18;
-float t25 = t21+t23-t28;
-float t29 = t20*t25;
-float t26 = tan(t29);
-float t27 = 1.0f/(t15*t15);
-float t30 = t26*t26;
-float t31 = t30+1.0f;
+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_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10));
-H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11));
-H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
+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));
\ No newline at end of file
diff --git a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
index 7af03eecca..1a200935e8 100644
--- a/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m	
+++ b/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m	
@@ -68,7 +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_MAGS real; % variance for magnetic deviation measurement
+syms R_YAW real; % variance for magnetic deviation measurement
 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)
@@ -269,19 +269,22 @@ 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 magnetic heading measurement
+%% derive equations for fusion of direct yaw measurement
 
-% rotate magnetic field into earth axes
-magMeasNED = Tbn*[magX;magY;magZ];
-% the predicted measurement is the angle wrt magnetic north of the horizontal
-% component of the measured field
-angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl;
-H_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian
+% rotate X body axis into earth axes
+yawVec = Tbn*[1;0;0];
+% Calculate the yaw angle of the projection
+angMeas = atan(yawVec(2)/yawVec(1));
+H_YAW = jacobian(angMeas,stateVector); % 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_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
+H_YAW = simplify(H_YAW);
 %[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
-ccode(H_MAGS,'file','calcH_MAGS.c');
+ccode(H_YAW,'file','calcH_YAW.c');
+% Calculate Kalman gain vector
+K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW);
+%K_MAGS = simplify(K_MAGS);
+ccode(K_YAW,'file','calcK_YAW.c');
 
 %% derive equations for fusion of synthetic deviation measurement
 % used to keep correct heading when operating without absolute position or
@@ -290,7 +293,7 @@ ccode(H_MAGS,'file','calcH_MAGS.c');
 magMeasNED = [magN;magE;magD];
 % the predicted measurement is the angle wrt magnetic north of the horizontal
 % component of the measured field
-angMeas = tan(magMeasNED(2)/magMeasNED(1));
+angMeas = atan(magMeasNED(2)/magMeasNED(1));
 H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
 H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
 H_MAGD = simplify(H_MAGD);