forked from Archive/PX4-Autopilot
8 lines
269 B
C
8 lines
269 B
C
|
A0[0][0] = q1*vd*2.0+q0*ve*2.0-q3*vn*2.0;
|
||
|
|
A0[0][1] = q0*vd*2.0-q1*ve*2.0+q2*vn*2.0;
|
||
|
|
A0[0][2] = q3*vd*2.0+q2*ve*2.0+q1*vn*2.0;
|
||
|
|
A0[0][3] = q2*vd*2.0-q3*ve*2.0-q0*vn*2.0;
|
||
|
|
A0[0][4] = q0*q3*-2.0+q1*q2*2.0;
|
||
|
|
A0[0][5] = q0*q0-q1*q1+q2*q2-q3*q3;
|
||
|
|
A0[0][6] = q0*q1*2.0+q2*q3*2.0;
|