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.
57 lines
1.5 KiB
57 lines
1.5 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 t2 = q0*q0; |
|
float t3 = q1*q1; |
|
float t4 = q2*q2; |
|
float t5 = q3*q3; |
|
float t6 = q0*q3*2.0f; |
|
float t8 = t2-t3+t4-t5; |
|
float t9 = q0*q1*2.0f; |
|
float t10 = q2*q3*2.0f; |
|
float t11 = t9-t10; |
|
float t14 = q1*q2*2.0f; |
|
float t21 = magY*t8; |
|
float t22 = t6+t14; |
|
float t23 = magX*t22; |
|
float t24 = magZ*t11; |
|
float t7 = t21+t23-t24; |
|
float t12 = t2+t3-t4-t5; |
|
float t13 = magX*t12; |
|
float t15 = q0*q2*2.0f; |
|
float t16 = q1*q3*2.0f; |
|
float t17 = t15+t16; |
|
float t18 = magZ*t17; |
|
float t19 = t6-t14; |
|
float t25 = magY*t19; |
|
float t20 = t13+t18-t25; |
|
if (fabsf(t20) < 1e-6f) { |
|
return; |
|
} |
|
float t26 = 1.0f/(t20*t20); |
|
float t27 = t7*t7; |
|
float t28 = t26*t27; |
|
float t29 = t28+1.0; |
|
if (fabsf(t29) < 1e-12f) { |
|
return; |
|
} |
|
float t30 = 1.0f/t29; |
|
if (fabsf(t20) < 1e-12f) { |
|
return; |
|
} |
|
float t31 = 1.0f/t20; |
|
|
|
// calculate observation jacobian |
|
float H_DECL[3] = {}; |
|
H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19)); |
|
H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17)); |
|
H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
|
|
|