Python ENU坐标系转WGS84坐标系

已知 enu 坐标的原点 wgs0 的情况下，进行 enu 坐标到 wgs84 坐标的转换

import math

# WGS84 ellipsoid parameters
WGS_PARAMS = {
    'a': 6378137,
    'f': 1 / 298.257223563,
    'b': 6378137 * (1 - 1 / 298.257223563),
}

def deg2radian(deg):
    return (deg * math.pi) / 180.0

def radian2deg(rad):
    return (rad * 180.0) / math.pi

def wgs2ecef(wgs):
    lng_r = deg2radian(wgs['lng'])
    lat_r = deg2radian(wgs['lat'])

    a = WGS_PARAMS['a']
    b = WGS_PARAMS['b']
    N = a ** 2 / math.sqrt(a ** 2 * math.cos(lat_r) ** 2 + b ** 2 * math.sin(lat_r) ** 2)

    x = (N + wgs['alt']) * math.cos(lat_r) * math.cos(lng_r)
    y = (N + wgs['alt']) * math.cos(lat_r) * math.sin(lng_r)
    z = (N * (b / a) ** 2 + wgs['alt']) * math.sin(lat_r)

    return {'x': x, 'y': y, 'z': z}

def ecef2wgs(x, y, z):
    a = WGS_PARAMS['a']
    b = WGS_PARAMS['b']
    e = math.sqrt(a ** 2 - b ** 2)

    r = math.sqrt(x ** 2 + y ** 2 + z ** 2)
    u = math.sqrt(0.5 * (r ** 2 - e ** 2) + 0.5 * math.sqrt((r ** 2 - e ** 2) ** 2 + 4 * e ** 2 * z ** 2))
    Q = math.hypot(x, y)
    huE = math.hypot(u, e)

    Beta = math.atan((huE / u) * (z / Q))
    eps = ((b * u - a * huE + e ** 2) * math.sin(Beta)) / ((a * huE * 1) / math.cos(Beta) - e ** 2 * math.cos(Beta))
    Beta += eps

    lat = math.atan((a / b) * math.tan(Beta))
    lng = math.atan2(y, x)
    alt = math.hypot(z - b * math.sin(Beta), Q - a * math.cos(Beta))

    inside = (x ** 2 / a ** 2 + y ** 2 / a ** 2 + z ** 2 / b ** 2) < 1
    if inside:
        alt = -alt

    lat = radian2deg(lat)
    lng = radian2deg(lng)

    return {'lat': lat, 'lng': lng, 'alt': alt}

def enu2uvw(east, north, up, lng0, lat0):
    lng_r = deg2radian(lng0)
    lat_r = deg2radian(lat0)

    t = math.cos(lat_r) * up - math.sin(lat_r) * north
    w = math.sin(lat_r) * up + math.cos(lat_r) * north
    u = math.cos(lng_r) * t - math.sin(lng_r) * east
    v = math.sin(lng_r) * t + math.cos(lng_r) * east

    return {'u': u, 'v': v, 'w': w}

def enu2ecef(enu, wgs0):
    ecef0 = wgs2ecef(wgs0)
    uvw = enu2uvw(enu['x'], enu['y'], enu['z'], wgs0['lng'], wgs0['lat'])

    return {
        'x': ecef0['x'] + uvw['u'],
        'y': ecef0['y'] + uvw['v'],
        'z': ecef0['z'] + uvw['w'],
    }

def enu2wgs(enu, wgs0):
    ecef = enu2ecef(enu, wgs0)
    return ecef2wgs(ecef['x'], ecef['y'], ecef['z'])

# Example usage
# enu = {'x': -16.47, 'y': 8.667, 'z': 0}
# wgs0 = {'lng': 121.5442525290947, 'lat': 31.229345976263158, 'alt': 0}
# result = enu2wgs(enu, wgs0)
# print(result)