EKF: Improve efficiency of yaw angle fusion

Use direct calculation of Kaman gains with optimised algebra

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,41 +527,43 @@ 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));
-
-	// rotate body field magnetic field measurement into earth frame and compare horizontal vector with published declination to get yaw measurement
-	// TODO - enable use of an off-board yaw measurement
-	matrix::Dcm<float> R_to_earth(_state.quat_nominal);
-	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
-	float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
-
-	// calculate the innovation
-	matrix::Euler<float> euler(_state.quat_nominal);
-	float innovation = euler(2) - measured_yaw;
-
-	// wrap the innovation to the interval between +-pi
-	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) {
+	H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8));	// calculate observation jacobian
+
+	// calculate Kalman gains
+	float t16 = q0 * q1 * 2.0f;
+	float t29 = q2 * q3 * 2.0f;
+	float t17 = t16 - t29;
+	float t18 = t15 * t17;
+	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
+	float t39;
+
+	if (t38 >= R_YAW) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
 		_fault_status.bad_mag_hdg = false;
+		t39 = 1.0f / t38;
+		_heading_innov_var = t38;
 
 	} else {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
@@ -572,44 +574,55 @@ void Ekf::fuseHeading()
 		return;
 	}
 
-	float innovation_var_inv = 1.0f / innovation_var;
-
-	// calculate the kalman gains taking dvantage of the reduce size of H_YAW
-	float Kfusion[_k_num_states] = {};
-
-	// 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_YAW[column];
-		}
-
-		Kfusion[row] *= innovation_var_inv;
+	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);
+
+	/* we won't be using these states because we are doing heading fusion
+	Kfusion[16] = t39*(P[16][1]*t40+P[16][2]*t41);
+	Kfusion[17] = t39*(P[17][1]*t40+P[17][2]*t41);
+	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);
+	*/
+
+	// don't adjust these states if we are not using them
+	if (_control_status.flags.wind) {
+		Kfusion[22] = t39 * (P[22][1] * t40 + P[22][2] * t41);
+		Kfusion[23] = t39 * (P[23][1] * t40 + P[23][2] * t41);
 	}
 
-	// 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;
-		}
-	}
+	// rotate body field magnetic field measurement into earth frame and compare horizontal vector with published declination to get yaw measurement
+	// TODO - enable use of an off-board yaw measurement
+	matrix::Dcm<float> R_to_earth(_state.quat_nominal);
+	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
+	float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
 
-	// 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];
-			}
+	// calculate the innovation
+	matrix::Euler<float> euler(_state.quat_nominal);
+	float innovation = euler(2) - measured_yaw;
 
-			Kfusion[row] *= innovation_var_inv;
-		}
-	}
+	// wrap the innovation to the interval between +-pi
+	innovation = matrix::wrap_pi(innovation);
+	_heading_innov = innovation;
 
 	// innovation test ratio
-	_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
+	_yaw_test_ratio = sq(innovation) / (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) {
@@ -623,7 +636,7 @@ 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);
 		}
 
@@ -645,7 +658,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];
 		}
 	}