Misc_Kinematics.py 文件源码

def quaterion2Euler(q):

    #Compute estimated rotation matrix elements
    R11 = 2.*q[0]**2-1 + 2.*q[1]**2
    R21 = 2.*(q[1]*q[2] - q[0]*q[3])
    R31 = 2.*(q[1]*q[3] + q[0]*q[2])
    R32 = 2.*(q[2]*q[3] - q[0]*q[1])
    R33 = 2.*q[0]**2 - 1 + 2*q[3]**2

    phi = math.atan2(R32, R33 )*180/math.pi                         # Roll
    theta = -math.atan(R31 / math.sqrt(1-R31**2) )*180/math.pi      # Pitch
    psi = math.atan2(R21, R11 )*180/math.pi                         # Yaw

    return [phi,theta,psi]

#Define body frame later