EKF: Convert GPS yaw observation method to use SymPy generated code (#880)

* EKF: add test comparison program for GPS yaw fusion equations

* EKF: convert GPS yaw fusion method to use SymPy generated equations

* Replace if/else with simple if

* EKF: fix formatting of GPS yaw fusion auto-code comparison test

* EKF: Use  SparseVector type for GPS yaw fusion Jacobians

* EKF: Fix bug in GPS yaw derivation comparison check

* Add gps_yaw auto generated code file

Co-authored-by: kamilritz <kritz@ethz.ch>

EKF/gps_yaw_fusion.cpp

@@ -34,6 +34,7 @@
 /**
  * @file gps_yaw_fusion.cpp
  * Definition of functions required to use yaw obtained from GPS dual antenna measurements.
+ * Equations generated using EKF/python/ekf_derivation/main.py
  *
  * @author Paul Riseborough <p_riseborough@live.com.au>
  *
@@ -48,10 +49,10 @@
 void Ekf::fuseGpsYaw()
 {
 	// assign intermediate state variables
-	const float q0 = _state.quat_nominal(0);
+	const float &q0 = _state.quat_nominal(0);
-	const float q1 = _state.quat_nominal(1);
+	const float &q1 = _state.quat_nominal(1);
-	const float q2 = _state.quat_nominal(2);
+	const float &q2 = _state.quat_nominal(2);
-	const float q3 = _state.quat_nominal(3);
+	const float &q3 = _state.quat_nominal(3);
 
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 	const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);
@@ -68,91 +69,61 @@ void Ekf::fuseGpsYaw()
 	// calculate predicted antenna yaw angle
 	const float predicted_hdg =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
 
-	// calculate observation jacobian
+	// using magnetic heading process noise
-	float t2 = sinf(_gps_yaw_offset);
+	// TODO extend interface to use yaw uncertainty provided by GPS if available
-	float t3 = cosf(_gps_yaw_offset);
-	float t4 = q0*q3*2.0f;
-	float t5 = q0*q0;
-	float t6 = q1*q1;
-	float t7 = q2*q2;
-	float t8 = q3*q3;
-	float t9 = q1*q2*2.0f;
-	float t10 = t5+t6-t7-t8;
-	float t11 = t3*t10;
-	float t12 = t4+t9;
-	float t13 = t3*t12;
-	float t14 = t5-t6+t7-t8;
-	float t15 = t2*t14;
-	float t16 = t13+t15;
-	float t17 = t4-t9;
-	float t19 = t2*t17;
-	float t20 = t11-t19;
-	float t18 = (t20*t20);
-	if (t18 < 1e-6f) {
-		return;
-	}
-	t18 = 1.0f / t18;
-	float t21 = t16*t16;
-	float t22 = sq(t11-t19);
-	if (t22 < 1e-6f) {
-		return;
-	}
-	t22 = 1.0f/t22;
-	float t23 = q1*t3*2.0f;
-	float t24 = q2*t2*2.0f;
-	float t25 = t23+t24;
-	float t26 = 1.0f/t20;
-	float t27 = q1*t2*2.0f;
-	float t28 = t21*t22;
-	float t29 = t28+1.0f;
-	if (fabsf(t29) < 1e-6f) {
-		return;
-	}
-	float t30 = 1.0f/t29;
-	float t31 = q0*t3*2.0f;
-	float t32 = t31-q3*t2*2.0f;
-	float t33 = q3*t3*2.0f;
-	float t34 = q0*t2*2.0f;
-	float t35 = t33+t34;
-
-	float H_YAW[4];
-	H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
-	H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
-	H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
-	H_YAW[3] = t30*(t26*t32+t16*t22*t35);
-
-	// using magnetic heading tuning parameter
 	const float R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
 
-	// calculate the innovation and define the innovation gate
+	// calculate intermediate variables
-	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
+	const float HK0 = sinf(_gps_yaw_offset);
-	_heading_innov = predicted_hdg - measured_hdg;
+	const float HK1 = q0*q3;
-
+	const float HK2 = q1*q2;
-	// wrap the innovation to the interval between +-pi
+	const float HK3 = 2*HK0*(HK1 - HK2);
-	_heading_innov = wrap_pi(_heading_innov);
+	const float HK4 = cosf(_gps_yaw_offset);
-
+	const float HK5 = powf(q1, 2);
-	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
+	const float HK6 = powf(q2, 2);
-	// calculate the innovation variance
+	const float HK7 = powf(q0, 2) - powf(q3, 2);
-	float PH[4];
+	const float HK8 = HK4*(HK5 - HK6 + HK7);
-	_heading_innov_var = R_YAW;
+	const float HK9 = HK3 - HK8;
-
+	if (fabsf(HK9) < 1e-3f) {
-	for (unsigned row = 0; row <= 3; row++) {
+		return;
-		PH[row] = 0.0f;
-
-		for (uint8_t col = 0; col <= 3; col++) {
-			PH[row] += P(row,col) * H_YAW[col];
-		}
-
-		_heading_innov_var += H_YAW[row] * PH[row];
 	}
+	const float HK10 = 1.0F/HK9;
+	const float HK11 = HK4*q0;
+	const float HK12 = HK0*q3;
+	const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
+	const float HK14 = HK10*HK13;
+	const float HK15 = HK0*q0 + HK4*q3;
+	const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
+	const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
+	if (fabsf(HK17) < 1e-3f) {
+		return;
+	}
+	const float HK18 = 2/HK17;
+	// const float HK19 = 1.0F/(-HK3 + HK8);
+	const float HK19_inverse = -HK3 + HK8;
+	if (fabsf(HK19_inverse) < 1e-6f) {
+		return;
+	}
+	const float HK19 = 1.0F/HK19_inverse;
+	const float HK20 = HK4*q1;
+	const float HK21 = HK0*q2;
+	const float HK22 = HK13*HK19;
+	const float HK23 = HK0*q1 - HK4*q2;
+	const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
+	const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
+	const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
+	const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
+	const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
+	const float HK29 = 4/powf(HK17, 2);
+	const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
+	const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
+	// const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
 
 	// check if the innovation variance calculation is badly conditioned
-	if (_heading_innov_var >= R_YAW) {
+	_heading_innov_var = (-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
-		// the innovation variance contribution from the state covariances is not negative, no fault
-		_fault_status.flags.bad_hdg = false;
 
-	} else {
+	if (_heading_innov_var < R_YAW) {
-		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+	// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_hdg = true;
 
 		// we reinitialise the covariance matrix and abort this fusion step
@@ -161,29 +132,15 @@ void Ekf::fuseGpsYaw()
 		return;
 	}
 
-	const float heading_innov_var_inv = 1.f / _heading_innov_var;
+	_fault_status.flags.bad_hdg = false;
+	const float HK32 = HK18/_heading_innov_var;
 
-	// calculate the Kalman gains
+	// calculate the innovation and define the innovation gate
-	// only calculate gains for states we are using
+	const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-	Vector24f Kfusion;
+	_heading_innov = predicted_hdg - measured_hdg;
 
-	for (uint8_t row = 0; row <= 15; row++) {
+	// wrap the innovation to the interval between +-pi
-		for (uint8_t col = 0; col <= 3; col++) {
+	_heading_innov = wrap_pi(_heading_innov);
-			Kfusion(row) += P(row,col) * H_YAW[col];
-		}
-
-		Kfusion(row) *= heading_innov_var_inv;
-	}
-
-	if (_control_status.flags.wind) {
-		for (uint8_t row = 22; row <= 23; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row,col) * H_YAW[col];
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-	}
 
 	// innovation test ratio
 	_yaw_test_ratio = sq(_heading_innov) / (sq(innov_gate) * _heading_innov_var);
@@ -210,27 +167,29 @@ void Ekf::fuseGpsYaw()
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
+	// calculate observation jacobian
+	// Observation jacobian and Kalman gain vectors
+	SparseVector24f<0,1,2,3> Hfusion;
+	Hfusion.at<0>() = -HK16*HK18;
+	Hfusion.at<1>() = -HK18*HK24;
+	Hfusion.at<2>() = -HK18*HK25;
+	Hfusion.at<3>() = HK18*HK26;
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	Vector24f Kfusion;
+	Kfusion(0) = HK27*HK32;
+	Kfusion(1) = HK28*HK32;
+	Kfusion(2) = HK30*HK32;
+	Kfusion(3) = HK31*HK32;
+	for (unsigned row = 4; row <= 23; row++) {
+		Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
+	}
+
 	// apply covariance correction via P_new = (I -K*H)*P
 	// first calculate expression for KHP
 	// then calculate P - KHP
-	SquareMatrix24f KHP;
+	const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
-	float KH[4];
-
-	for (unsigned row = 0; row < _k_num_states; row++) {
-
-		KH[0] = Kfusion(row) * H_YAW[0];
-		KH[1] = Kfusion(row) * H_YAW[1];
-		KH[2] = Kfusion(row) * H_YAW[2];
-		KH[3] = Kfusion(row) * H_YAW[3];
-
-		for (unsigned column = 0; column < _k_num_states; column++) {
-			float tmp = KH[0] * P(0,column);
-			tmp += KH[1] * P(1,column);
-			tmp += KH[2] * P(2,column);
-			tmp += KH[3] * P(3,column);
-			KHP(row,column) = tmp;
-		}
-	}
 
 	const bool healthy = checkAndFixCovarianceUpdate(KHP);
 

EKF/python/ekf_derivation/generated/gps_yaw_fusion_generated_compare.cpp

@@ -0,0 +1,266 @@
+#include <math.h>
+#include <stdio.h>
+#include <cstdlib>
+#include "../../../../../matrix/matrix/math.hpp"
+
+typedef matrix::Vector<float, 24> Vector24f;
+typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;
+template<int ... Idxs>
+using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
+
+float sq(float in) {
+	return in * in;
+}
+
+int main()
+{
+	// Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations
+
+	float H_YAW[24];
+	Vector24f Kfusion;
+	float _heading_innov_var;
+
+	const float R_YAW = sq(0.3f);
+
+	const float _gps_yaw_offset = 1.5f;
+
+	// quaternion inputs must be normalised
+	float q0 = 2.0f * ((float)rand() - 0.5f);
+	float q1 = 2.0f * ((float)rand() - 0.5f);
+	float q2 = 2.0f * ((float)rand() - 0.5f);
+	float q3 = 2.0f * ((float)rand() - 0.5f);
+	const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
+	q0 /= length;
+	q1 /= length;
+	q2 /= length;
+	q3 /= length;
+
+	// create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
+	SquareMatrix24f P;
+	for (int col=0; col<=23; col++) {
+		for (int row=0; row<=col; row++) {
+			if (row == col) {
+				P(row,col) = (float)rand();
+			} else {
+				P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
+			}
+		}
+	}
+
+	// First calculate observationjacobians and Kalman gains using sympy generated equations
+
+	// calculate intermediate variables
+	const float HK0 = sinf(_gps_yaw_offset);
+	const float HK1 = q0*q3;
+	const float HK2 = q1*q2;
+	const float HK3 = 2*HK0*(HK1 - HK2);
+	const float HK4 = cosf(_gps_yaw_offset);
+	const float HK5 = powf(q1, 2);
+	const float HK6 = powf(q2, 2);
+	const float HK7 = powf(q0, 2) - powf(q3, 2);
+	const float HK8 = HK4*(HK5 - HK6 + HK7);
+	const float HK9 = HK3 - HK8;
+	if (fabsf(HK9) < 1e-3f) {
+		return 0;
+	}
+	const float HK10 = 1.0F/HK9;
+	const float HK11 = HK4*q0;
+	const float HK12 = HK0*q3;
+	const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
+	const float HK14 = HK10*HK13;
+	const float HK15 = HK0*q0 + HK4*q3;
+	const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
+	const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
+	if (fabsf(HK17) < 1e-3f) {
+		return 0;
+	}
+	const float HK18 = 2/HK17;
+	// const float HK19 = 1.0F/(-HK3 + HK8);
+	const float HK19_inverse = -HK3 + HK8;
+	if (fabsf(HK19_inverse) < 1e-6f) {
+		return 0;
+	}
+	const float HK19 = 1.0F/HK19_inverse;
+	const float HK20 = HK4*q1;
+	const float HK21 = HK0*q2;
+	const float HK22 = HK13*HK19;
+	const float HK23 = HK0*q1 - HK4*q2;
+	const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
+	const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
+	const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
+	const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
+	const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
+	const float HK29 = 4/powf(HK17, 2);
+	const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
+	const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
+	const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+
+	// calculate observation jacobian
+	// Observation jacobian and Kalman gain vectors
+	SparseVector24f<0,1,2,3> Hfusion;
+	Hfusion.at<0>() = -HK16*HK18;
+	Hfusion.at<1>() = -HK18*HK24;
+	Hfusion.at<2>() = -HK18*HK25;
+	Hfusion.at<3>() = HK18*HK26;
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	Kfusion(0) = HK27*HK32;
+	Kfusion(1) = HK28*HK32;
+	Kfusion(2) = HK30*HK32;
+	Kfusion(3) = HK31*HK32;
+	for (unsigned row = 4; row <= 23; row++) {
+		Kfusion(row) = HK32*(-HK16*P(0,row) - HK24*P(1,row) - HK25*P(2,row) + HK26*P(3,row));
+	}
+
+	// save output and repeat calculation using legacy matlab generated code
+	float Hfusion_sympy[24] = {};
+	Vector24f Kfusion_sympy;
+	Hfusion_sympy[0] = Hfusion.at<0>();
+	Hfusion_sympy[1] = Hfusion.at<1>();
+	Hfusion_sympy[2] = Hfusion.at<2>();
+	Hfusion_sympy[3] = Hfusion.at<3>();
+
+	for (int row=0; row<24; row++) {
+		Kfusion_sympy(row) = Kfusion(row);
+	}
+
+	// repeat calculation using matlab generated equations
+	// calculate observation jacobian
+	float t2 = sinf(_gps_yaw_offset);
+	float t3 = cosf(_gps_yaw_offset);
+	float t4 = q0*q3*2.0f;
+	float t5 = q0*q0;
+	float t6 = q1*q1;
+	float t7 = q2*q2;
+	float t8 = q3*q3;
+	float t9 = q1*q2*2.0f;
+	float t10 = t5+t6-t7-t8;
+	float t11 = t3*t10;
+	float t12 = t4+t9;
+	float t13 = t3*t12;
+	float t14 = t5-t6+t7-t8;
+	float t15 = t2*t14;
+	float t16 = t13+t15;
+	float t17 = t4-t9;
+	float t19 = t2*t17;
+	float t20 = t11-t19;
+	float t18 = (t20*t20);
+
+	t18 = 1.0f / t18;
+	float t21 = t16*t16;
+	float t22 = sq(t11-t19);
+
+	t22 = 1.0f/t22;
+	float t23 = q1*t3*2.0f;
+	float t24 = q2*t2*2.0f;
+	float t25 = t23+t24;
+	float t26 = 1.0f/t20;
+	float t27 = q1*t2*2.0f;
+	float t28 = t21*t22;
+	float t29 = t28+1.0f;
+
+	float t30 = 1.0f/t29;
+	float t31 = q0*t3*2.0f;
+	float t32 = t31-q3*t2*2.0f;
+	float t33 = q3*t3*2.0f;
+	float t34 = q0*t2*2.0f;
+	float t35 = t33+t34;
+
+	memset(&H_YAW, 0, sizeof(H_YAW));
+	H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
+	H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
+	H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
+	H_YAW[3] = t30*(t26*t32+t16*t22*t35);
+
+	// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
+	// calculate the innovation variance
+	float PH[4];
+	_heading_innov_var = R_YAW;
+
+	for (unsigned row = 0; row <= 3; row++) {
+		PH[row] = 0.0f;
+
+		for (uint8_t col = 0; col <= 3; col++) {
+			PH[row] += P(row,col) * H_YAW[col];
+		}
+
+		_heading_innov_var += H_YAW[row] * PH[row];
+	}
+
+	const float heading_innov_var_inv = 1.f / _heading_innov_var;
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	memset(&Kfusion, 0, sizeof(Kfusion));
+
+	for (uint8_t row = 0; row <= 15; row++) {
+		for (uint8_t col = 0; col <= 3; col++) {
+			Kfusion(row) += P(row,col) * H_YAW[col];
+		}
+		Kfusion(row) *= heading_innov_var_inv;
+	}
+
+	if (true) {
+		for (uint8_t row = 22; row <= 23; row++) {
+			for (uint8_t col = 0; col <= 3; col++) {
+				Kfusion(row) += P(row,col) * H_YAW[col];
+			}
+			Kfusion(row) *= heading_innov_var_inv;
+		}
+	}
+
+	// find largest observation variance difference as a fraction of the matlab value
+	float max_diff_fraction = 0.0f;
+	int max_row;
+	float max_old, max_new;
+	for (int row=0; row<24; row++) {
+		float diff_fraction;
+		if (H_YAW[row] != 0.0f) {
+			diff_fraction = fabsf(Hfusion_sympy[row] - H_YAW[row]) / fabsf(H_YAW[row]);
+		} else if (Hfusion_sympy[row] != 0.0f) {
+			diff_fraction = fabsf(Hfusion_sympy[row] - H_YAW[row]) / fabsf(Hfusion_sympy[row]);
+		} else {
+			diff_fraction = 0.0f;
+		}
+		if (diff_fraction > max_diff_fraction) {
+			max_diff_fraction = diff_fraction;
+			max_row = row;
+			max_old = H_YAW[row];
+			max_new = Hfusion_sympy[row];
+		}
+	}
+
+	if (max_diff_fraction > 1E-5f) {
+		printf("Fail: GPS yaw Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+	} else {
+		printf("Pass: GPS yaw Hfusion max diff fraction = %e\n",max_diff_fraction);
+	}
+
+	// find largest Kalman gain difference as a fraction of the matlab value
+	max_diff_fraction = 0.0f;
+	for (int row=0; row<4; row++) {
+		float diff_fraction;
+		if (Kfusion(row) != 0.0f) {
+			diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row));
+		} else if (Kfusion_sympy(row) != 0.0f) {
+			diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row));
+		} else {
+			diff_fraction = 0.0f;
+		}
+		if (diff_fraction > max_diff_fraction) {
+			max_diff_fraction = diff_fraction;
+			max_row = row;
+			max_old = Kfusion(row);
+			max_new = Kfusion_sympy(row);
+		}
+	}
+
+	if (max_diff_fraction > 1E-5f) {
+		printf("Fail: GPS yaw Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
+	} else {
+		printf("Pass: GPS yaw Kfusion max diff fraction = %e\n",max_diff_fraction);
+	}
+
+    return 0;
+}

EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp

@@ -0,0 +1,90 @@
+// Sub Expressions
+const float HK0 = sinf(ant_yaw);
+const float HK1 = q0*q3;
+const float HK2 = q1*q2;
+const float HK3 = 2*HK0*(HK1 - HK2);
+const float HK4 = cosf(ant_yaw);
+const float HK5 = powf(q1, 2);
+const float HK6 = powf(q2, 2);
+const float HK7 = powf(q0, 2) - powf(q3, 2);
+const float HK8 = HK4*(HK5 - HK6 + HK7);
+const float HK9 = HK3 - HK8;
+const float HK10 = 1.0F/HK9;
+const float HK11 = HK4*q0;
+const float HK12 = HK0*q3;
+const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2);
+const float HK14 = HK10*HK13;
+const float HK15 = HK0*q0 + HK4*q3;
+const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15);
+const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1;
+const float HK18 = 2/HK17;
+const float HK19 = 1.0F/(-HK3 + HK8);
+const float HK20 = HK4*q1;
+const float HK21 = HK0*q2;
+const float HK22 = HK13*HK19;
+const float HK23 = HK0*q1 - HK4*q2;
+const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23);
+const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23);
+const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15);
+const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3);
+const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3);
+const float HK29 = 4/powf(HK17, 2);
+const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3);
+const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3);
+const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW);
+
+
+// Observation Jacobians
+Hfusion.at<0>() = -HK16*HK18;
+Hfusion.at<1>() = -HK18*HK24;
+Hfusion.at<2>() = -HK18*HK25;
+Hfusion.at<3>() = HK18*HK26;
+Hfusion.at<4>() = 0;
+Hfusion.at<5>() = 0;
+Hfusion.at<6>() = 0;
+Hfusion.at<7>() = 0;
+Hfusion.at<8>() = 0;
+Hfusion.at<9>() = 0;
+Hfusion.at<10>() = 0;
+Hfusion.at<11>() = 0;
+Hfusion.at<12>() = 0;
+Hfusion.at<13>() = 0;
+Hfusion.at<14>() = 0;
+Hfusion.at<15>() = 0;
+Hfusion.at<16>() = 0;
+Hfusion.at<17>() = 0;
+Hfusion.at<18>() = 0;
+Hfusion.at<19>() = 0;
+Hfusion.at<20>() = 0;
+Hfusion.at<21>() = 0;
+Hfusion.at<22>() = 0;
+Hfusion.at<23>() = 0;
+
+
+// Kalman gains
+Kfusion(0) = HK27*HK32;
+Kfusion(1) = HK28*HK32;
+Kfusion(2) = HK30*HK32;
+Kfusion(3) = HK31*HK32;
+Kfusion(4) = HK32*(-HK16*P(0,4) - HK24*P(1,4) - HK25*P(2,4) + HK26*P(3,4));
+Kfusion(5) = HK32*(-HK16*P(0,5) - HK24*P(1,5) - HK25*P(2,5) + HK26*P(3,5));
+Kfusion(6) = HK32*(-HK16*P(0,6) - HK24*P(1,6) - HK25*P(2,6) + HK26*P(3,6));
+Kfusion(7) = HK32*(-HK16*P(0,7) - HK24*P(1,7) - HK25*P(2,7) + HK26*P(3,7));
+Kfusion(8) = HK32*(-HK16*P(0,8) - HK24*P(1,8) - HK25*P(2,8) + HK26*P(3,8));
+Kfusion(9) = HK32*(-HK16*P(0,9) - HK24*P(1,9) - HK25*P(2,9) + HK26*P(3,9));
+Kfusion(10) = HK32*(-HK16*P(0,10) - HK24*P(1,10) - HK25*P(2,10) + HK26*P(3,10));
+Kfusion(11) = HK32*(-HK16*P(0,11) - HK24*P(1,11) - HK25*P(2,11) + HK26*P(3,11));
+Kfusion(12) = HK32*(-HK16*P(0,12) - HK24*P(1,12) - HK25*P(2,12) + HK26*P(3,12));
+Kfusion(13) = HK32*(-HK16*P(0,13) - HK24*P(1,13) - HK25*P(2,13) + HK26*P(3,13));
+Kfusion(14) = HK32*(-HK16*P(0,14) - HK24*P(1,14) - HK25*P(2,14) + HK26*P(3,14));
+Kfusion(15) = HK32*(-HK16*P(0,15) - HK24*P(1,15) - HK25*P(2,15) + HK26*P(3,15));
+Kfusion(16) = HK32*(-HK16*P(0,16) - HK24*P(1,16) - HK25*P(2,16) + HK26*P(3,16));
+Kfusion(17) = HK32*(-HK16*P(0,17) - HK24*P(1,17) - HK25*P(2,17) + HK26*P(3,17));
+Kfusion(18) = HK32*(-HK16*P(0,18) - HK24*P(1,18) - HK25*P(2,18) + HK26*P(3,18));
+Kfusion(19) = HK32*(-HK16*P(0,19) - HK24*P(1,19) - HK25*P(2,19) + HK26*P(3,19));
+Kfusion(20) = HK32*(-HK16*P(0,20) - HK24*P(1,20) - HK25*P(2,20) + HK26*P(3,20));
+Kfusion(21) = HK32*(-HK16*P(0,21) - HK24*P(1,21) - HK25*P(2,21) + HK26*P(3,21));
+Kfusion(22) = HK32*(-HK16*P(0,22) - HK24*P(1,22) - HK25*P(2,22) + HK26*P(3,22));
+Kfusion(23) = HK32*(-HK16*P(0,23) - HK24*P(1,23) - HK25*P(2,23) + HK26*P(3,23));
+
+