GPS卫星定位—python实现

 # coding:utf-8

    
 import os
    
 import math as m
    
 with open('brdm0320.22p','r') as f:
    
     data=f.readlines()
    
     # print(data)
    
     f.close()
    
 #文件时间不同可取消此处注释
    
 # def o_data_line():
    
 #     for i in range(len(data)):
    
 #         if data[i].find('END OF HEADER') != -1:
    
 #             o_data_line = i + 1
    
 #     return o_data_line
    
 # print(o_data_line())
    
 ff=[]
    
 ff=data[207:3359]
    
 # print(ff)
    
  
    
 file_write_obj = open("GPS_data.txt", 'w')   # 新文件写入GPS_data
    
 for i in range(3359-207) :
    
     file_write_obj.write(ff[i])   # 逐行写入
    
     # file_write_obj.write('\n')
    
 file_write_obj.close()
    
 print("保存文件成功")
    
  
    
 #文件读取
    
 with open('GPS_data.txt','r') as G:
    
  
    
     GPS_data=G.readlines()
    
     f.close()
    
     # print(GPS_data)
    
 data_num=int(len(GPS_data)/8)         #数据组数
    
 # print(data_num)
    
  
    
  
    
 PRN = []
    
 year = []
    
 day=[]
    
 month=[]
    
 hour = []
    
 minute=[]
    
 second=[]
    
 S=[]
    
 S_s=[]
    
 S_ss=[]
    
 IODE=[]
    
 C_rs=[]
    
 n=[]
    
 mo=[]
    
 C_uc=[]
    
 e=[]
    
 C_us=[]
    
 sqrt_A=[]
    
 TEO=[]
    
 C_ic=[]
    
 C_is=[]
    
 OMEGA=[]
    
 I_0=[]
    
 C_rc=[]
    
 w=[]
    
 OMEGA_DOT=[]
    
 IDOT=[]
    
 L2_code = []
    
 PS_week_num=[]
    
 L2_P_code=[]
    
 TGD=[]
    
 IODC=[]
    
 for j in range(data_num):
    
  
    
     for i in range(8):
    
     data_l = GPS_data[8 * j +i]
    
     if i==0:
    
         # 卫星PRN号
    
  
    
         PRN.append(data_l[0:3])
    
  
    
         # 历元 TIME
    
  
    
         TIME=data_l[4:23]
    
  
    
         #year = []
    
         year.append(int(TIME[0:4]))
    
         #month=[]
    
         month.append(int(TIME[5:7]))
    
         #day=[]
    
         day.append(int(TIME[8:10]))
    
         #hour = []
    
         hour.append(int(TIME[11:13]))
    
         #minute=[]
    
         minute.append(int(TIME[14:16]))
    
         #second=[]
    
         second.append(float(TIME[17:19]))
    
  
    
         # print(TIME)
    
         # 卫星钟偏差(s)
    
         #S=[]
    
         S.append(float(data_l[23:42]))
    
  
    
  
    
        #卫星钟漂(s/s)
    
         #S_s=[]
    
         S_s.append(float(data_l[42:62]))
    
         # print(type(S_s))
    
         # print(S_s)
    
  
    
     # 卫星钟漂移速度(s / s * s)
    
         #S_ss=[]
    
         S_ss.append(float(data_l[62:81]))
    
  
    
     elif i==1:
    
         #IODE=[]
    
         IODE.append(float(data_l[4:23]))
    
         #C_rs=[]
    
         C_rs.append(float(data_l[23:42]))
    
         #n=[]
    
         n.append(float(data_l[43:61]))
    
         #mo=[]
    
         mo.append(float(data_l[61:81]))
    
     elif i==2:
    
         #C_uc=[]
    
         C_uc.append(float(data_l[4:23]))
    
         # print(C_uc)
    
         # print(float(row[4:23]))
    
         #e=[]
    
         e.append(float(data_l[23:42]))
    
         #C_us=[]
    
         C_us.append(float(data_l[42:62]))
    
         #sqrt_A=[]
    
         sqrt_A.append(float(data_l[62:81]))
    
     elif i == 3:
    
         #TEO=[]
    
         TEO.append(float(data_l[4:23]))
    
         #C_ic=[]
    
         C_ic.append(float(data_l[23:42]))
    
         #OMEGA=[]
    
         OMEGA.append(float(data_l[42:61]))
    
         #C_is=[]
    
         C_is.append(float(data_l[62:81]))
    
     elif i== 4:
    
         #I_0=[]
    
         I_0.append(float(data_l[4:23]))
    
         #C_rc=[]
    
         C_rc.append(float(data_l[23:42]))
    
         #w=[]
    
         w.append(float(data_l[43:61]))
    
         #OMEGA_DOT=[]
    
         OMEGA_DOT.append(float(data_l[62:81]))
    
     elif i == 5:
    
         #IDOT=[]
    
         IDOT .append(float(data_l[4:23]))
    
         #L2_code=[]
    
         L2_code .append(float(data_l[23:42]))
    
         #PS_week_num=[]
    
         PS_week_num .append(float(data_l[42:62]))
    
         #L2_P_code=[]
    
         L2_P_code .append(float(data_l[62:81]))
    
     elif i == 6:
    
         # 卫星精度(m)=2
    
         # 卫星健康状态=0
    
         #TGD=[]
    
         TGD .append(float(data_l[42:62]))
    
         #IODC=[]
    
         IODC .append(float(data_l[62:81]))
    
         # print(IODC)
    
     elif i==7:
    
         pass
    
 # print(PRN, S,S_s,S_ss,
    
 #       IODE,C_rs,n,mo, C_uc,
    
 #       e, C_us, sqrt_A,
    
 #       TEO, C_ic, OMEGA,C_is,
    
 #       I_0, C_rc, w, OMEGA_DOT,
    
 #       IDOT,L2_code,PS_week_num,L2_P_code,
    
 #       TGD,IODC)
    
  
    
  
    
 def CulLocation(PRN, year, month, day, hour, minute, second, S, S_s, S_ss, IDOT, C_rs, n, mo, C_uc, e, C_us, sqrt_A,
    
             TEO, C_ic, OMEGA, C_is, I_0, C_rc, w, OMEGA_DOT,t1):
    
  
    
  
    
     # 1.计算卫星运行平均角速度 GM:WGS84下的引力常数 =3.986005e14，a:长半径
    
     GM = 398600500000000
    
     n_0 = m.sqrt(GM) / m.pow(sqrt_A, 3)
    
     n = n_0 + n
    
  
    
     # 2.计算归化时间t_k 计算t时刻的卫星位置  UT：世界时 此处以小时为单位
    
     UT = hour + (minute / 60.0) + (second / 3600);
    
  
    
     if month <= 2:
    
     year = year - 1
    
     month = month + 12  # 1，2月视为前一年13，14月
    
  
    
     # 需要将当前需计算的时刻先转换到儒略日再转换到GPS时间
    
     JD = (365.25 * year) + int(30.6001 * (month + 1)) + day + UT / 24 + 1720981.5;
    
     WN = int((JD - 2444244.5) / 7);  # WN:GPS_week number 目标时刻的GPS周
    
     t_oc = (JD - 2444244.5 - 7.0 * WN) * 24 * 3600.0;  # t_GPS:目标时刻的GPS秒
    
  
    
     # 对观测时刻t1进行钟差改正,注意：t1应是由接收机接收到的时间
    
     if t1 is None:
    
     t_k = 0
    
     else:
    
     δt = a_0 + a_1(t1 - t_oc) + a_2(t1 - t_oc) ^ 2
    
     t = t1 - δt
    
     t_k = t - TEO
    
  
    
     if t_k > 302400:
    
     t_k -= 604800
    
     else :
    
     t_k += 604800
    
  
    
     # 3.平近点角计算M_k = M_0+n*t_k
    
     # print(mo)
    
     M_k = mo + n * t_k  # 实际应该是乘t_k，但是没有接收机的观测时间，所以为了练手设t_k=0
    
  
    
     # 4.偏近点角计算 E_k  (迭代计算) E_k = M_k + e*sin(E_k)
    
     E = 0;
    
     E1 = 1;
    
     count = 0;
    
     while abs(E1 - E) > 1e-10:
    
     count = count + 1
    
     E1 = E;
    
     E = M_k + e * m.sin(E);
    
     if count > 1e8:
    
         print("计算偏近点角时未收敛！")
    
         break
    
  
    
         # 5.计算卫星的真近点角
    
     V_k = m.atan((m.sqrt(1 - e * e) * m.sin(E)) / (m.cos(E) - e));
    
  
    
     # 6.计算升交距角 u_0(φ_k), ω:卫星电文给出的近地点角距
    
     u_0 = V_k + w
    
  
    
     # 7.摄动改正项 δu、δr、δi :升交距角u、卫星矢径r和轨道倾角i的摄动量
    
     δu = C_uc * m.cos(2 * u_0) + C_us * m.sin(2 * u_0)
    
     δr = C_rc * m.cos(2 * u_0) + C_rs * m.sin(2 * u_0)
    
     δi = C_ic * m.cos(2 * u_0) + C_is * m.sin(2 * u_0)
    
  
    
     # 8.计算经过摄动改正的升交距角u_k、卫星矢径r_k和轨道倾角 i_k
    
     u = u_0 + δu
    
     r = m.pow(sqrt_A, 2) * (1 - e * m.cos(E)) + δr
    
     i = I_0 + δi + IDOT * (t_k);  # 实际乘t_k=t-t_oe
    
  
    
     # 9.计算卫星在轨道平面坐标系的坐标,卫星在轨道平面直角坐标系（X轴指向升交点）中的坐标为：
    
     x_k = r * m.cos(u)
    
     y_k = r * m.sin(u)
    
  
    
     # 10.观测时刻升交点经度Ω_k的计算，升交点经度Ω_k等于观测时刻升交点赤经Ω与格林尼治恒星时GAST之差  Ω_k=Ω_0+(ω_DOT-omega_e)*t_k-omega_e*t_oe
    
     omega_e = 7.292115e-5  # 地球自转角速度
    
     OMEGA_k = OMEGA + (OMEGA_DOT - omega_e) * t_k - omega_e * TEO;  # 星历中给出的Omega即为Omega_o=Omega_t_oe-GAST_w
    
  
    
     # 11.计算卫星在地固系中的直角坐标l
    
     X_k = x_k * m.cos(OMEGA_k) - y_k * m.cos(i) * m.sin(OMEGA_k)
    
     Y_k = x_k * m.sin(OMEGA_k) + y_k * m.cos(i) * m.cos(OMEGA_k)
    
     Z_k = y_k * m.sin(i)
    
  
    
     # 12.计算卫星在协议地球坐标系中的坐标，考虑级移
    
     # [X,Y,Z]=[[1,0,X_P],[0,1,-Y_P],[-X_p,Y_P,1]]*[X_k,Y_k,Z_k]
    
     # XYZ=[X,Y,Z]
    
  
    
     if month > 12:  # 恢复历元
    
     year = year + 1
    
     month = month - 12
    
  
    
     print("历元：", year, "年", month, "月", day, "日", hour, "时", minute, "分", second, "秒", "\n卫星PRN号:", PRN, "\n平均角速度:", n,
    
       "\n卫星平近点角:", M_k, "\n偏近点角:", E, "\n真近点角:", V_k, "\n升交距角:", u_0, "\n摄动改正项:", δu, δr, δi, "\n经摄动改正后的升交距角、卫星矢径和轨道倾角:", u, r,
    
       i, "\n轨道平面坐标X,Y:", x_k, y_k, "\n观测时刻升交点经度:", OMEGA_k, "\n地固直角坐标系X:", X_k, "\n地固直角坐标系Y:", Y_k, "\n地固直角坐标系Z:", Z_k,'\n地固坐标系的位置')
    
  
    
  
    
     with open('GPS_XYZ.txt', 'a') as P:
    
     P.write(str(X_k) )
    
     P.write('   ')
    
     P.write(str(Y_k) )
    
     P.write('   ')
    
     P.write(str(Z_k) )
    
     P.write('\n')
    
 if __name__ == '__main__':
    
  
    
     with open("GPS_XYZ.txt", 'r+') as file:#清空文件
    
     file.truncate(0)
    
     for i in range(data_num-1):
    
         t1=None
    
         CulLocation(PRN[i], year[i], month[i], day[i], hour[i], minute[i], second[i], S[i], S_s[i], S_ss[i], IDOT[i], C_rs[i], n[i], mo[i], C_uc[i], e[i], C_us[i],
    
                 sqrt_A[i], TEO[i],
    
                 C_ic[i], OMEGA[i], C_is[i], I_0[i], C_rc[i], w[i], OMEGA_DOT[i],t1)