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

3 lines
68 B
C

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