You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
42 lines
1.2 KiB
42 lines
1.2 KiB
/* |
|
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;
|
|
|