px4-firmware/matlab/scripts/Inertial Nav EKF/yaw_input_321.c

3 lines
69 B
C
Raw Normal View History

A0[0][0] = q0*q3*-2.0+q1*q2*2.0;
A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;