GPS卫星定位—python实现

  基于导航文件的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)

由于我近期在完成GNSS课程的一个作业，无论是专业知识还是编程语言都是初学阶段，本着提高自己的原则， 借鉴网上一些大神的资料，完成了本次作业。主要是借鉴了python读取导航电文并计算卫星位置_Small Cube的博客-CSDN博客_python卫星定位

该作者给与了很大帮助 ，经过该作者同意，上传本人第一个CSDN作品！！！

但是此次由于搞出来太激动了还未来得及完成结果准确性的检验，会在后续继续补充完善，如果有什么不妥，请各位大佬批评指正。