GPS转化ECEF坐标系(0) python和c++代码 - MKT-porter

随笔- 1525 文章- 2 评论- 66 阅读- 128万

GPS转化ECEF坐标系(0) python和c++代码

https://www.cxyzjd.com/article/taiyang1987912/112982150

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

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)