WGS-84坐标系下大地坐标blh和空间直角坐标系xyz互转，以及转站心坐标系neu
# -*- coding: utf-8 -*-"""Created on Tue Dec 1 17:06:01 @author: uuu"""import matplotlibimport matplotlib.pyplot as pltimport numpy as npimport mathdef blh2xyz(blh):#大地坐标转空间直角坐标a = 6378137.0f=1.0/298.257223563e=math.sqrt(2*f-f*f);e2 = 0.00669437999013lat = blh[0]lon = blh[1]height = blh[2]slat = np.sin(lat)clat = np.cos(lat)slon = np.sin(lon)clon = np.cos(lon)t2lat = (np.tan(lat))*(np.tan(lat))tmp = 1 - e*etmpden = np.sqrt(1 + tmp * t2lat)tmp2 = np.sqrt(1 - e*e*slat*slat)N=a/tmp2x = (N+height)*clat*clony = (N+height)*clat*slonz = (a*tmp*slat) / tmp2 + height * slatreturn [x,y,z]def xyz2enu( xyz, orgblh):#空间直角坐标转站心坐标系lat = orgblh[0]lon = orgblh[1]height = orgblh[2]slat = np.sin(lat)clat = np.cos(lat)slon = np.sin(lon)clon = np.cos(lon)tmpxyz=[0,0,0]orgxyz=[0,0,0]tmporg=[0,0,0]difxyz= [0,0,0]enu=[0,0,0]orgxyz=blh2xyz(orgblh)for i in range(3):tmpxyz[i] = xyz[i]tmporg[i] = orgxyz[i]difxyz[i] = tmpxyz[i] - tmporg[i]R_list = [[-slon,clon,0] , [-slat * clon,-slat * slon,clat ], [clat*clon,clat*slon,slat ] ]for i in range(3):enu[0] = enu[0] + R_list[0][i] * difxyz[i]enu[1] = enu[1] + R_list[1][i] * difxyz[i]enu[2] = enu[2] + R_list[2][i] * difxyz[i]return enudef xyz2blh(xyz): # 空间直角坐标转换为大地坐标blh=[0,0,0]# 长半轴a = 6378137.0# 扁率f = 1.0/298.257223563e2=f*(2-f)r2=xyz[0]*xyz[0]+xyz[1]*xyz[1]z=xyz[2]zk=0.0while(abs(z-zk)>=0.0001):zk=z;sinp=z/math.sqrt(r2+z*z);v=a/math.sqrt(1.0-e2*sinp*sinp);z=xyz[2]+v*e2*sinp;if(r2>1E-12):blh[0]=math.atan(z/math.sqrt(r2))blh[1]=math.atan2(xyz[1],xyz[0])else:if(r2>0):blh[0]=math.pi/2.0else:blh[0]=-math.pi/2.0blh[1]=0.0blh[2]=math.sqrt(r2+z*z)-vreturn blh# mainif __name__ == "__main__":snxPos=[2919785.79290941,-5383744.95894879,1774604.86526307]blh0=xyz2blh(snxPos)print(blh0)xyz2=blh2xyz(blh0)print(xyz2)
有部分代码是参考别人的博客，现在找不到原链接了。还有一部分是参考rtklib
