forked from Archive/PX4-Autopilot
68 lines
1.6 KiB
Plaintext
68 lines
1.6 KiB
Plaintext
/*
|
|
Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
|
|
*/
|
|
|
|
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
|
|
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;
|
|
}
|
|
|
|
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;
|
|
|
|
/*
|
|
Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
|
|
*/
|
|
|
|
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
|
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;
|
|
}
|
|
|
|
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;
|