https://www.cxyzjd.com/article/taiyang1987912/112982150
import math
a = 6378137
b = 6356752.3142
f = (a - b) / a
e_sq = f * (2-f)
pi = 3.14159265359
'''
功能： # gps转化到大地坐标系ECEF
输入：
等待转换的GPS 坐标 lat, lon, h
输出：
ecef 坐标 x, y, z
'''
def GPSwgs84ToEcef(lat, lon, h):
# (lat, lon) in degrees
# h in meters
lamb = math.radians(lat)
phi = math.radians(lon)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x = (h + N) * cos_lambda * cos_phi
y = (h + N) * cos_lambda * sin_phi
z = (h + (1 - e_sq) * N) * sin_lambda
XYZ=[x,y,z]
return XYZ
'''
功能： # 大地坐标系 转化到GPS第一帧为原点的本地ENU坐标系
输入：
等待转换的ecef 坐标 x, y, z
作为原点的GPS第一帧 坐标lat0, lon0, h0
输出：
本地第一帧GPS为原点的 ENU 坐标系 xEast, yNorth, zUp
'''
def GPSecef_to_enu(x, y, z, lat0, lon0, h0):
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
xd = x - x0
yd = y - y0
zd = z - z0
t = -cos_phi * xd - sin_phi * yd
xEast = -sin_phi * xd + cos_phi * yd
yNorth = t * sin_lambda + cos_lambda * zd
zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd
return xEast, yNorth, zUp
'''
功能： enu坐标转化到ecef坐标
输入：
等待转换的ENU坐标 坐标 xEast, yNorth, zUp
GPS第一帧原点 坐标 lat0, lon0, h0
输出：
ecef 坐标 x, y, z
'''
def enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0):
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
t = cos_lambda * zUp - sin_lambda * yNorth
zd = sin_lambda * zUp + cos_lambda * yNorth
xd = cos_phi * t - sin_phi * xEast
yd = sin_phi * t + cos_phi * xEast
x = xd + x0
y = yd + y0
z = zd + z0
return x, y, z
'''
功能： # 大地坐标系ECEF转化到gps
输入：
等待转换的ecef 坐标 x, y, z
输出：
GPS 坐标 lat, lon, h
'''
def ecef_to_geodetic(x, y, z):
# Convert from ECEF cartesian coordinates to
# latitude, longitude and height. WGS-84
x2 = x ** 2
y2 = y ** 2
z2 = z ** 2
a = 6378137.0000 # earth radius in meters
b = 6356752.3142 # earth semiminor in meters
e = math.sqrt (1-(b/a)**2)
b2 = b*b
e2 = e ** 2
ep = e*(a/b)
r = math.sqrt(x2+y2)
r2 = r*r
E2 = a ** 2 - b ** 2
F = 54*b2*z2
G = r2 + (1-e2)*z2 - e2*E2
c = (e2*e2*F*r2)/(G*G*G)
s = ( 1 + c + math.sqrt(c*c + 2*c) )**(1/3)
P = F / (3 * (s+1/s+1)**2 * G*G)
Q = math.sqrt(1+2*e2*e2*P)
ro = -(P*e2*r)/(1+Q) + math.sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2)
tmp = (r - e2*ro) ** 2
U = math.sqrt( tmp + z2 )
V = math.sqrt( tmp + (1-e2)*z2 )
zo = (b2*z)/(a*V)
height = U*( 1 - b2/(a*V) )
lat = math.atan( (z + ep*ep*zo)/r )
temp = math.atan(y/x)
if x >=0 :
long = temp
elif (x < 0) & (y >= 0):
long = pi + temp
else :
long = temp - pi
lat0 = lat/(pi/180)
lon0 = long/(pi/180)
h0 = height
return lat0, lon0, h0
'''
功能： # gps直接转化到enu坐标系
相对于指定GPS_ref为原点（一般都是第一帧）的enu坐标系
输入：
等待转换的GPS 坐标 lat, lon, h
参考原点GPS 坐标 lat_ref, lon_ref, h_ref
输出：
enu坐标 x, y, z
'''
def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
x, y, z = geodetic_to_ecef(lat, lon, h)
return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)
'''
功能： # enu直接转化到gps坐标系
相对于指定GPS_ref为原点（一般都是第一帧）的enu坐标系
输入：
等待转换的enu 坐标 xEast, yNorth, zUp
参考原点GPS 坐标 lat_ref, lon_ref, h_ref
输出：
GPS 坐标 lat, lon, h
'''
def enu_to_geodetic(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref):
x,y,z = enu_to_ecef(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref)
return ecef_to_geodetic(x,y,z)
标签：ECEF,phi,python,cos,c++,ref,sin,math,lambda From： https://www.cnblogs.com/gooutlook/p/17041240.html