dcm.py 文件源码

def _to_hpr_single(dcm):
    pitch = np.arcsin(dcm[2, 1])
    if np.abs(pitch) < 0.5 * np.pi - 1e-3:
        heading = np.arctan2(dcm[0, 1], dcm[1, 1])
        roll = np.arctan2(-dcm[2, 0], dcm[2, 2])
    elif pitch > 0:
        roll = 0
        heading = np.arctan2(-dcm[0, 2] - dcm[1, 0], dcm[0, 0] - dcm[1, 2])
    else:
        roll = 0
        heading = np.arctan2(dcm[0, 2] - dcm[1, 0], dcm[0, 0] + dcm[1, 2])

    if heading < 0:
        heading += 2 * np.pi

    if heading == 2 * np.pi:
        heading = 0

    return heading, pitch, roll