def quat2yaw(q):
    return np.arctan2((2 * ((q.y * q.z) + (q.w * q.x))), (1 - (2 * ((q.z ** 2) + (q.w ** 2)))))
def quat2yaw(q):
    return np.arctan2((2 * ((q.y * q.z) + (q.w * q.x))), (1 - (2 * ((q.z ** 2) + (q.w ** 2)))))