GPS标准单点定位代码.zip

# -*- coding: utf-8 -*- """ Created on Tue May 21 15:53:13 2019 @author: 72987 """ # -*- coding: utf-8 -*- """ Created on Mon May 13 21:13:29 2019 @author: 72987 """ import timetranslate import GPSreadfile import numpy as np import math import coorsystem class cal_main (object): Tc1_time,sat_pos_temp,satpostion,obs_time,trtime=[],[],[],[],[] c=2.99792458e8 we=7.2921150e-5#地球自转角速度 GM=3.986004418e14 sat_pos=[] satpostion1=[] sat_pos_temp1=[] sat_pos=sat_pos_temp obstime=trtime sametimesat=[] satpostion_t=[] x1=GPSreadfile.Openreadfile.oHeadTemp['StationPositionXX'] y1=GPSreadfile.Openreadfile.oHeadTemp['StationPositionYY'] z1=GPSreadfile.Openreadfile.oHeadTemp['StationPositionZZ'] StationXYZ=[] StationXYZ.append(x1) StationXYZ.append(y1) StationXYZ.append(z1) station000=StationXYZ ######################计算卫星位置############### def calculate_satpos (sat_pos,obstime_week,obstime_sec): GM=3.986004418e14 we=7.2921150e-5#地球自转角速度 Cis=0 SAT_POS=[] #卫星参数 #a0=float(sat_pos[7].strip()) ##a2=float(sat_pos[9].strip()) #IODE=float(sat_pos[10].strip()) #a0=float(sat_pos[7].strip()) ##a2=float(sat_pos[9].strip()) #IODE=float(sat_pos[10].strip()) Crs=float(sat_pos[11].strip()) delta_n=float(sat_pos[12].strip()) M0=float(sat_pos[13].strip()) Cuc=float(sat_pos[14].strip()) e=float(sat_pos[15].strip()) Cus=float(sat_pos[16].strip()) sqrtA=float(sat_pos[17].strip()) Toe_sec=float(sat_pos[18].strip()) Cic=float(sat_pos[19].strip()) OMEGA=float(sat_pos[20].strip()) Cis==float(sat_pos[21].strip()) CIS=Cis i0=float(sat_pos[22].strip()) Crc=float(sat_pos[23].strip()) W=float(sat_pos[24].strip()) OMEGA_DOT=float(sat_pos[25].strip()) i_DOT=float(sat_pos[26].strip()) #Code_L2=float(sat_pos[27].strip()) Toe_week=float(sat_pos[28].strip()) #TGD=float(sat_pos[32].strip()) #IODC=float(sat_pos[33].strip()) #print(obstime) t2_week,t2_sec=obstime_week,obstime_sec #计算参考时间 delta_t = t2_sec-Toe_sec+(t2_week-Toe_week)*604800 #print(delta_t) if delta_t>302400: delta_t=delta_t-604800 elif delta_t<-302400: delta_t=delta_t+604800 #计算卫星运动的平均角速度n n = math.sqrt(GM)/sqrtA**3 + delta_n #计算观测瞬间卫星的平近点角M M = M0 + n*delta_t #计算偏近点角E E0=M E=M+e*math.sin(E0) while (E-E0) > 1e-12: E0=E E=M+e*math.sin(E0) #计算真近点角f f = math.atan2((math.sqrt(1-e**2)* math.sin(E)),(math.cos(E)-e)) #计算升交角距 ut = W + f #计算摄动改正项Ou,Or,Oi Ou = Cuc*math.cos(2*ut) + Cus*math.sin(2*ut) Or = Crc*math.cos(2*ut) + Crs*math.sin(2*ut) Oi = Cic*math.cos(2*ut) + CIS*math.sin(2*ut) #对ut,rt,i0进行摄动改正 u = ut + Ou r = sqrtA**2*(1-e*math.cos(E)) + Or i = i0 + i_DOT*delta_t+Oi #计算卫星在轨道面坐标系中的位置 xgd = r*math.cos(u) ygd = r*math.sin(u) #计算观测瞬间升交点的经度L L=OMEGA+(OMEGA_DOT-we)*delta_t-we*Toe_sec #计算卫星在瞬时地球坐标系中的位置 X = xgd *math.cos(L) - ygd *math.cos(i)*math.sin(L) Y = xgd *math.sin(L) + ygd *math.cos(i)*math.cos(L) Z = ygd *math.sin(i) SAT_POS.append(X) SAT_POS.append(Y) SAT_POS.append(Z) return X,Y,Z ######################寻找最佳星历######################## def find_bestfittime(oprn,nprn,nfiledata,obs_time,trtime): max1=10000 ngtime_t=[] for j in range(len(nprn)): if oprn in nprn[j]: ngtime_t=nfiledata[j*36:j*36+7] nyear=int(ngtime_t[1].strip()) nmonth=int(ngtime_t[2].strip()) nday=int(ngtime_t[3].strip()) nhour=int(ngtime_t[4].strip()) nmin=int(ngtime_t[5].strip()) nsec=int(ngtime_t[6].strip()) ngtime=timetranslate.time(nyear,nmonth,nday,nhour,nmin,nsec) if(abs(ngtime[1]-obs_time[1])<max1): max1=abs(ngtime[1]-obs_time[1]) pn=j if (max1==10000): pn=-1 return pn ######################地球自转改正##################### def earthrot(satpostion,delta_t): PI=3.14 omiga=PI*2.0/(24.0*3600) alpha=omiga*delta_t cl=math.cos(alpha) sl=math.sin(alpha) R=np.array([[cl,sl,0],[-sl,cl,0],[0,0,1]]) Xx=np.array([satpostion[0],satpostion[1],satpostion[2]]) v=np.zeros((1,3)) postion=[] i=0 j=0 for i in range(len(satpostion)): v[0][i]=0 for j in range(len(satpostion)): v[0][i]=R[j][i]*Xx[j]+v[0][i] postion.append(v[0][0]) postion.append(v[0][1]) postion.append(v[0][2]) return postion ###################对流层改正#################### def troposphere(Bp,Lp,X,Y,Z,earthrot): x,y,z=earthrot.x,earthrot.y,earthrot.z pi=3.1415926535898 Nb=-math.sin(Bp)*math.cos(Lp)*(X-x)-math.sin(Bp)*math.sin(Lp)*(Y-y)+math.cos(Bp)*(Z-z) Eb=-math.sin(Lp)*(X-x)+math.cos(Lp)*(Y-y) Ub=math.cos(Bp)*math.cos(Lp)*(X-x)+math.cos(Bp)*math.sin(Lp)*(Y-y) El=math.atan(Ub/math.sqrt(math.pow(Nb,2)+math.pow(Eb,2))) if(El<0): El=El+pi/2 trop=2.47/(math.sin(El)+0.0121) return trop #####################电离层############################ def ion(el,Fp,Lp,UT,a0,a1,a2,b0,b1,b2,a): EA=(math.radians(445)/el+math.radians(20))-math.radians(4) Fp_t=Fp+EA*math.cos(a) Lp_t=Lp+EA*math.sin(a)/math.cos(Fp) t=UT+Lp_t/15 Fm=Fp_t+math.radians(10.07)*math.cos(Lp_t-math.radians(228.04)) A=a0*Fm+a1*Fm+a2*Fm P=b0*Fm+b1*Fm+b2*Fm Ts=5e-9+A*math.cos(2*math.pi/P*(t-14)) sZ=1+2*(math.radians(96)-el)/math.radians(90) Ts_t=Ts*sZ return Ts_t #####################卫星高度角################## def sathighetradions(stationpos,satpos): rad=[] NEU=coorsystem.CARTtoTOP(stationpos,satpos) Ele=abs(math.atan(NEU[2]/math.sqrt(NEU[0]**2+NEU[1]**2))) Azi=math.atan(abs(NEU[1]/NEU[0])) if NEU[0]>0: if NEU[1]<0: Azi=2*math.pi-Azi else: if NEU[1]>0: Azi=math.pi-Azi else: Azi=math.pi+Azi rad.append(Ele) rad.append(Azi) return rad ###########################法方程#####