car.py 文件源码

def ecef2geodetic(x, y, z):
    """http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm

    This algorithm provides a converging solution to the latitude equation in
    terms of the parametric or reduced latitude form (v).
    This algorithm provides a uniform solution over all latitudes as it does
    not involve division by cos(phi) or sin(phi).
    """
    ell = {}
    ell['a'] = 6378137.
    ell['f'] = 1. / 298.2572235630
    ell['b'] = ell['a'] * (1 - ell['f'])

    ea = ell['a']
    eb = ell['b']
    rad = np.hypot(x, y)
    # Constant required for Latitude equation
    rho = np.arctan2(eb * z, ea * rad)
    #Constant required for latitude equation
    c = (ea**2 - eb**2) / np.hypot(ea * rad, eb * z)
    # Starter for the Newtons Iteration Method
    vnew = np.arctan2(ea * z, eb * rad)
    # Initializing the parametric latitude
    v = 0
    count = 0
    while (v != vnew).any() and count < 5:
        v = vnew.copy()
        # Newtons Method for computing iterations
        vnew = v - ((2 * np.sin(v - rho) - c * np.sin(2 * v)) /
                    (2 * (np.cos(v - rho) - c * np.cos(2 * v))))
        count += 1

    # Computing latitude from the root of the latitude equation
    lat = np.arctan2(ea * np.tan(vnew), eb)
    lon = np.arctan2(y, x)
    alt = ((rad - ea * np.cos(vnew)) * np.cos(lat) +
           (z - eb * np.sin(vnew)) * np.sin(lat))

    return lat, lon, alt