/* 
Observation jacobian for fusion of the horizontal components of magnetometer measurements.

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.

This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use

Divide by zero protection hs been added
*/

// calculate intermediate variables for observation jacobian
float t9 = q0*q3;
float t10 = q1*q2;
float t2 = t9+t10;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = t3+t4-t5-t6;
float t8 = t7*t7;
if (t8 > 1e-6f) {
	t8 = 1.0f/t8;
} else {
	return;
}
float t11 = t2*t2;
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
	t14 = 1.0f/t13;
} else {
	return;
}

// calculate observation jacobian
float H_DECL[4] = {};
H_YAW[0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0f)*-2.0f;
H_YAW[1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0f)*-2.0f;
H_YAW[2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0f)*2.0f;
H_YAW[3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0f)*2.0f;