PX4-Autopilot/doc/euler_gimbal_lock.ipynb

In [4]:

from sympy import *
init_printing()

In [16]:

th1, th2, th3 = symbols('theta_1, theta_2, theta_3')
dcm_321 = Matrix([[cos(th1)*cos(th2),
         cos(th1)*sin(th2)*sin(th3) - sin(th1)*cos(th3),
         cos(th1)*sin(th2)*cos(th3) + sin(th1)*sin(th3)],
        [sin(th1)*cos(th2),
         sin(th1)*sin(th2)*sin(th3) + cos(th1)*cos(th3),
         sin(th1)*sin(th2)*cos(th3) - cos(th1)*sin(th3)],
        [-sin(th2),
        cos(th2)*sin(th3),
        cos(th2)*cos(th3)]
    ])
dcm_321

Out [16]:

In [18]:

phi, theta, psi = symbols('phi, theta, psi')
sub_eulers = {
    th1: psi, th2: theta, th3: phi
}
dcm_321_euler = dcm_321.subs(sub_eulers)
dcm_321_euler

Out [18]:

In [38]:

dcm_pitch_up = dcm_321_euler.subs({theta:pi/2, phi:0})
dcm_pitch_up

Out [38]:

In [39]:

atan2(dcm_pitch_up[1,2], dcm_pitch_up[0,2])

Out [39]:

In [40]:

dcm_pitch_down = dcm_321_euler.subs({theta:pi/2, phi:0})
dcm_pitch_down

Out [40]:

In [41]:

dcm_pitch_down = dcm_321_euler.subs({theta:-pi/2, phi:0})
atan2(-dcm_pitch_down[1,2], -dcm_pitch_down[0,2])

Out [41]: