forked from Archive/PX4-Autopilot
17 lines
325 B
C
17 lines
325 B
C
t2 = q0*q0;
|
|
t3 = q1*q1;
|
|
t4 = q2*q2;
|
|
t5 = q3*q3;
|
|
t6 = t2-t3+t4-t5;
|
|
t7 = q0*q3*2.0;
|
|
t10 = q1*q2*2.0;
|
|
t8 = t7-t10;
|
|
t9 = 1.0/(t6*t6);
|
|
t11 = t8*t8;
|
|
t12 = t9*t11;
|
|
t13 = t12+1.0;
|
|
t14 = 1.0/t13;
|
|
t15 = 1.0/t6;
|
|
A0[0][0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
|
|
A0[0][2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
|