util.py 文件源码

def BodyRatesToEarthRates(dcm, gyro):
    """Convert the angular velocities from body frame to
    earth frame.

    all inputs and outputs are in radians/s

    returns a earth rate vector.
    """
    from math import sin, cos, tan, fabs

    p      = gyro.x
    q      = gyro.y
    r      = gyro.z

    (phi, theta, psi) = dcm.to_euler()

    phiDot   = p + tan(theta) * (q * sin(phi) + r * cos(phi))
    thetaDot = q * cos(phi) - r * sin(phi)
    if fabs(cos(theta)) < 1.0e-20:
        theta += 1.0e-10
    psiDot   = (q * sin(phi) + r * cos(phi)) / cos(theta)
    return Vector3(phiDot, thetaDot, psiDot)