mirror of https://github.com/ArduPilot/ardupilot
39 lines
1.1 KiB
C++
39 lines
1.1 KiB
C++
|
float t3 = q0*q0;
|
||
|
|
float t4 = q1*q1;
|
||
|
|
float t5 = q2*q2;
|
||
|
|
float t6 = q3*q3;
|
||
|
|
float t2 = t3+t4+t5+t6;
|
||
|
|
float t7 = t2*t2;
|
||
|
|
float t11 = q0*q3*2.0f;
|
||
|
|
float t12 = q1*q2*2.0f;
|
||
|
|
float t8 = t11-t12;
|
||
|
|
float t13 = q0*q2*2.0f;
|
||
|
|
float t14 = q1*q3*2.0f;
|
||
|
|
float t9 = t13+t14;
|
||
|
|
float t10 = t3+t4-t5-t6;
|
||
|
|
float t15 = q0*q1*2.0f;
|
||
|
|
float t16 = t11+t12;
|
||
|
|
float t17 = dvxNoise*t10*t16;
|
||
|
|
float t18 = t3-t4+t5-t6;
|
||
|
|
float t19 = q2*q3*2.0f;
|
||
|
|
float t20 = t15-t19;
|
||
|
|
float t21 = t15+t19;
|
||
|
|
float t22 = t3-t4-t5+t6;
|
||
|
|
float t23 = t13-t14;
|
||
|
|
float t24 = dvzNoise*t9*t22;
|
||
|
|
float t25 = t24-dvxNoise*t10*t23-dvyNoise*t8*t21;
|
||
|
|
float t26 = dvyNoise*t18*t21;
|
||
|
|
float t27 = t26-dvxNoise*t16*t23-dvzNoise*t20*t22;
|
||
|
|
A0[0][0] = daxNoise*t7;
|
||
|
|
A0[1][1] = dayNoise*t7;
|
||
|
|
A0[2][2] = dazNoise*t7;
|
||
|
|
A0[3][3] = dvxNoise*(t10*t10)+dvyNoise*(t8*t8)+dvzNoise*(t9*t9);
|
||
|
|
A0[3][4] = t17-dvzNoise*t9*(t15-q2*q3*2.0f)-dvyNoise*t8*t18;
|
||
|
|
A0[3][5] = t25;
|
||
|
|
A0[4][3] = t17-dvyNoise*t8*t18-dvzNoise*t9*t20;
|
||
|
|
A0[4][4] = dvxNoise*(t16*t16)+dvyNoise*(t18*t18)+dvzNoise*(t20*t20);
|
||
|
|
A0[4][5] = t27;
|
||
|
|
A0[5][3] = t25;
|
||
|
|
A0[5][4] = t27;
|
||
|
|
A0[5][5] = dvxNoise*(t23*t23)+dvyNoise*(t21*t21)+dvzNoise*(t22*t22);
|