frame.py 文件源码

def _geodetic_to_cartesian(lat, lon, alt):
    """Conversion from latitude, longitue and altitude coordinates to
    cartesian with respect to an ellipsoid

    Args:
        lat (float): Latitude in radians
        lon (float): Longitue in radians
        alt (float): Altitude to sea level in meters

    Return:
        numpy.array: 3D element (in meters)
    """
    C = Earth.r / np.sqrt(1 - (Earth.e * np.sin(lat)) ** 2)
    S = Earth.r * (1 - Earth.e ** 2) / np.sqrt(1 - (Earth.e * np.sin(lat)) ** 2)
    r_d = (C + alt) * np.cos(lat)
    r_k = (S + alt) * np.sin(lat)

    norm = np.sqrt(r_d ** 2 + r_k ** 2)
    return norm * np.array([
        np.cos(lat) * np.cos(lon),
        np.cos(lat) * np.sin(lon),
        np.sin(lat)
    ])