conversions.py 文件源码

def q_to_euler(q):
    """Converts Quaternions to Euler angles.

    Parameters
    ----------
    q : array_like
        Array holding Quaternions.

    Returns
    -------
    phi : float
        `phi` angle in radians.
    theta :float
        `theta` angle in radians.
    psi : float
        `psi` angle in radians.
    """

    phi = np.arctan2(2*(q[0]*q[1]+q[2]*q[3]),(q[0]**2+q[3]**2-q[1]**2-q[2]**2))
    theta = np.arcsin(2*(q[0]*q[2]-q[1]*q[3]))
    psi = np.arctan2(2*(q[0]*q[3]+q[1]*q[2]),(q[0]**2+q[1]**2-q[2]**2-q[3]**2))

    return phi, theta, psi