0
0

【Python】伪距单点定位

一、算法原理

1.关于伪距单点定位

GPS伪距单点定位的原理比较简单，主要是利用空间距离的后方交会，用一台接收机同时接受四颗卫星的位置坐标和卫星与接收机的距离，运用后方交会原理解算出接收机的三维坐标。其中，如果接收机观测的卫星的数目多于四颗，则采用最小二乘法进行平差计算，求解出接收机坐标。

2.伪距单点定位的具体原理

（1）伪距观测方程

若得到了测站的近似坐标，则可以使用泰勒展开得到线性化方程如下。

（2）列写伪距观测方程，线性化后，得到误差方程：

（3）计算信号发射时刻卫星i的位置与信号到达时接收机的近似距离之间的距离。

3.伪距单点定位程序的计算步骤

1.读取RINEX N文件，将所有星历放置到一个列表ephlst（数组）中。

2.读取RINEX O文件，读取一个历元观测值epoch。

3.数据预处理

根据数组中的卫星号和历元时刻T在ephlst查找相应的卫星星历，

准则

4.程序初始化，设置测站概略位置为Xr，接受机钟差dtr。

14.输出该历元定位结果

二、程序设计

1.读取文件及数据准备

  1. # 数据处理
  2. import numpy
  4. def donfile(path):
  5.     with open(path, 'r') as f:
  6.         # 获取行数
  7.         lines_n = f.readlines()
  8.     Ndata = []
  9.     # 获取数据组数
  10.     data_line_num = int(len(lines_n) / 8)
  11.     print(data_line_num)
  12.     Ndataitemkeys = ['IODE', 'Crs', 'Delta_n', 'M0', 'Cuc', 'e', 'Cus', 'sqrt_A', 'TEO', 'Cic', 'OMEGA_A0', 'Cis', 'i0',
  13.                      'Crc',
  14.                      'omega', 'OMEGA_DOT', 'IDOT', 'L2Codes', 'GPSWEEK', 'L2PCODE', '卫星精度', '卫星健康状态', 'TGD', 'IODC',
  15.                      '电文发送时刻', '拟合区间', '备用1', '备用2']
  16.     # 遍历
  17.     for i in range(data_line_num):
  18.         Ndataitem = {}
  19.         k = 0
  20.         for j in range(8):
  21.             data_countent = lines_n[8 * i + j]
  22.             Ndataitem['数据组号'] = i + 1
  23.             if j == 0:
  24.                 Ndataitem['卫星PRN号'] = data_countent.strip('\n')[0:3]
  25.                 Ndataitem['历元'] = data_countent.strip('\n')[4:23]
  26.                 I = data_countent.strip('\n')[23:42]  # [:-4]+ 'e' + data_countent.strip('\n')[24:42][-3:]
  27.                 Ndataitem['卫星钟差参数s'] = str(float(
  28.                     (data_countent.strip('\n')[23:42][:-4].strip()) + 'e' + data_countent.strip('\n')[23:42][-3:]))
  29.                 Ndataitem['卫星钟漂参数s/s'] = str(float(data_countent.strip('\n')[42:61][:-4].strip()) * numpy.power(10.0, -int( data_countent.strip(  '\n')[42:61][-2:])))
  30.                 Ndataitem['卫星钟漂速度参数s/s/s'] = str( float(data_countent.strip('\n')[61:80][:-4].strip()) * numpy.power(10.0,int(data_countent.strip('\n')[62:80][-2:])))
  32.                 IB = data_countent.strip('\n')[2:22][-4]  # 1-1.731250000000
  34.             elif 0 < j < 7:
  35.                 if data_countent.strip('\n')[4:23][-3] == '-':
  36.                     Ndataitem[Ndataitemkeys[k]] = str(
  37.                         float(data_countent.strip('\n')[4:23][:-4].strip()) * numpy.power(10.0, -int(
  38.                             data_countent.strip('\n')[4:23][-2:])))
  39.                 else:
  40.                     Ndataitem[Ndataitemkeys[k]] = str(
  41.                         float(data_countent.strip('\n')[4:23][:-4].strip()) * numpy.power(10.0,
  42.                                                                                           int(data_countent.strip('\n')[
  43.                                                                                               4:23][-2:])))
  44.                 if data_countent.strip('\n')[23:42][-3] == '-':
  45.                     Ndataitem[Ndataitemkeys[k + 1]] = str(
  46.                         float(data_countent.strip('\n')[23:42][:-4].strip()) * numpy.power(10.0, -int(
  47.                             data_countent.strip('\n')[23:42][-2:])))
  48.                 # IB = data_countent.strip('\n')[2:22][-4]  # 1-1.731250000000
  49.                 else:
  50.                     B = data_countent.strip('\n')[23:42][:-4]
  51.                     Ndataitem[Ndataitemkeys[k + 1]] = str(
  52.                         float(data_countent.strip('\n')[23:42][:-4].strip()) * numpy.power(10.0, int(data_countent.strip('\n')[23:42][-2:])))
  53.                 if data_countent.strip('\n')[42:61][-3] == '-':
  54.                     Ndataitem[Ndataitemkeys[k + 2]] = str(float(data_countent.strip('\n')[42:61][:-4].strip()) * numpy.power(10.0, -int(data_countent.strip('\n')[42:61][-2:])))
  55.                 else:
  56.                     Ndataitem[Ndataitemkeys[k + 2]] = str(
  57.                         float(data_countent.strip('\n')[42:61][:-4].strip()) * numpy.power(10.0,int(data_countent.strip( '\n')[42:61][-2:])))
  58.                 if data_countent.strip('\n')[61:80][-3] == '-':
  59.                     Ndataitem[Ndataitemkeys[k + 3]] = str(float(data_countent.strip('\n')[61:80][:-4].strip()) * numpy.power(10.0, -int(data_countent.strip('\n')[61:80][-2:])))
  60.                 else:
  61.                     Ndataitem[Ndataitemkeys[k + 3]] = str( float(data_countent.strip('\n')[61:80][:-4].strip()) * numpy.power(10.0, int(data_countent.strip('\n')[61:80][-2:])))
  62.                 k = k + 4
  63.         if len(Ndataitem) > 4:
  64.             Ndata.append(Ndataitem)
  66.     return Ndata
  67. def head_num(lines_n):
  68.     for i in range(len(lines_n)):
  69.         if lines_n[i].find('END OF HEADER') != -1:
  70.             data_num = i + 1
  71.     return data_num

2.卫星位置以及迭代计算

  1. import math
  2. import  numpy
  3. from numpy import *
  5. def nfilecompute(Ndata):
  7.     #计算xyz的三维坐标
  8.     xyzs = []
  9.     A1=[]
  10.     L1=[]
  11.     #初值测站坐标-2424425.1013  5377188.1768  2418617.7454
  12.     #初值化
  13.     X0=mat([[0],[0],[0],[0]])
  14.     sum=1
  16.     #导入C1码数据即伪距观测值
  17.     C1=[21937747.840,24275893.980,21322965.800,22097191.620,24075984.740, 21068789.740,20803383.780 ]
  18.     C2 = [float(1/21937747.84), float(1/24275893.98), float(1/21322965.8), float(1/24606230.92), float(1/22097191.62),float( 1/24075984.74), float(1/21068789.74)]
  20.     while True:
  21.         for ii in range(len(Ndata)):
  22.             xyz = []
  23.             #系数矩阵
  24.             A=[]
  26.             c = 299792458 # 光速常数
  27.             u = 3.986004418e14#地球引力常数
  28.             a = numpy.power(float(Ndata[ii]['sqrt_A']) ,2)#卫星轨道长半径a
  30.             n0 = numpy.sqrt(u/numpy.power(a , 3))#参考时刻TOE平均角速度n0
  32.             n = n0 + float(Ndata[ii]['Delta_n'])#时刻未定的平均角速度n
  34.             e = float(Ndata[ii]['e'])#椭圆轨道偏心率
  36.             we = 7.2921151467e-5
  37.             cal=[]
  38.             cal.append(int(Ndata[ii]['历元'][:4])) #年
  39.             cal.append(int(Ndata[ii]['历元'][5:7]))#月
  40.             cal.append(int(Ndata[ii]['历元'][9:10]))#日
  41.             cal.append(int(Ndata[ii]['历元'][12:13]))#时
  42.             cal.append(int(Ndata[ii]['历元'][14:16]))#分
  43.             cal.append(int(Ndata[ii]['历元'][17:19]))#秒
  45.             #计算参考时刻的时间
  47.             t_oc = cal2gps(cal)#将UTC转换为GPS周内秒
  49.             #计算观测时间(历元时刻）
  51.             t=cal2gps([2022,3,9,0,0])
  53.             #卫星钟差改正
  54.             a_0=float(Ndata[ii]['卫星钟差参数s'])
  55.             a_1=float(Ndata[ii]['卫星钟漂参数s/s'])
  56.             a_2=float(Ndata[ii]['卫星钟漂速度参数s/s/s'])
  57.             #观测时刻2022,3,9,0,0，计算卫星钟差δt
  58.             δt = a_0 + a_1*(t[1] - t_oc[1]) + a_2*((t[1]-t_oc[1])^2)
  59.             # dtr = C1[ii] / c  #dtr
  60.             #接收机钟差
  61.             dtr = float(X0[3][0])/c
  62.             # print(dtr)
  63.             dtsi = δt  # 卫星钟差
  65.             #while True:
  67.             # 选择数据中的一颗卫星的观测值设伪距为C1[ii](需要迭代）
  68.             # 计算卫星Sii的信号发射的概略时刻，Tsii
  69.             # (a)计算卫星Sii的信号传播时间dtsii
  71.             dtsii = C1[ii] / c - dtr + dtsi
  73.             Tr = t[1]  # Tr历元时刻
  74.             # (b)计算卫星Sii的信号发射时刻Tsii
  76.             Tsii = Tr - dtsii
  78.             t2 = Tsii - δt
  80.                 #计算dtr
  81.             while True:
  82.                 tk = t2 - float(Ndata[ii]['TEO'])  # 规划时间
  84.                 M = float(Ndata[ii]['M0']) + n*tk#观测卫星瞬间平近点角M  TOE参考时刻  t接收机接收卫星信号时刻（变量）
  85.                 E = computeE(M,e)#利用迭代方式解算偏近点角E
  86.                 f = 2 * math.atan(numpy.sqrt((1+e)/(1-e))*math.tan(E/2))
  87.                 _theta = f + float(Ndata[ii]['omega'])
  88.                 g_theta , g_r , g_i = sdgz(float(Ndata[ii]['Cuc']) , float(Ndata[ii]['Cus']) , float(Ndata[ii]['Crc']) , float(Ndata[ii]['Crs']) , float(Ndata[ii]['Cic']) , float(Ndata[ii]['Cis']) , _theta)
  89.                 theta = _theta + g_theta
  90.                 r = a * (1 - e * math.cos(E)) + g_r
  91.                 i = float(Ndata[ii]['i0']) + g_i + float(Ndata[ii]['IDOT']) * tk
  92.                 x = r * math.cos(theta)#卫星在轨道平面坐标系中的x
  93.                 y = r * math.sin(theta)#卫星在轨道平面坐标系中的y
  94.                 OMEGA = float(Ndata[ii]['OMEGA_A0']) + (float(Ndata[ii]['OMEGA_DOT']) - we)*t[1] - (float(Ndata[ii]['OMEGA_DOT']) * float(Ndata[ii]['TEO']))
  95.                #计算地心地固坐标
  96.                 X1 = x * math.cos(OMEGA) - y * math.cos(i) * math.sin(OMEGA)
  97.                 Y1 = x * math.sin(OMEGA) + y * math.cos(i) * math.cos(OMEGA)
  98.                 Z1 = y * math.sin(i)
  99.                 #初始化，置测站概略位置为xyz_0,接受机钟差初值为dtr
  100.                 #xyz_0_coord = [0,0,0]
  101.                 x_0 = float(X0[0][0])
  102.                 y_0 = float(X0[1][0])
  103.                 z_0 = float(X0[2][0])
  105.                 # (c)计算卫星sii在Tsi的位置
  107.                 #计算发射时刻的卫星坐标(近似坐标)，并且对卫星坐标进行地球自传改正
  108.                 tch=C1[ii]/c
  109.                 X=X1*cos(we*tch)+Y1*sin(we*tch)
  110.                 Y=-sin(we*tch)*X1+Y1*cos(we*tch)
  111.                 Z=Z1
  112.                 #计算近似卫星在接受时刻的坐标
  114.                 #计算卫星和测站的近似几何距离
  115.                 disct0 = numpy.sqrt(numpy.power((X - x_0), 2) + numpy.power((Y - y_0), 2) + numpy.power((Z- z_0), 2))
  116. #几何距离 求信号传播时间
  118.                 dts1ii=disct0/c
  119.                 g=dts1ii-dtsii
  121.                 if(abs(dts1ii-dtsii)<10.0e-7):
  122.                     break
  123.                 else:
  124.                     dtsii=dts1ii
  126.                     sum=sum+1
  128.             #rs
  130.             b0=-(X-x_0)/disct0
  131.             b1=-(Y-y_0)/disct0
  132.             b2=-(Z-z_0)/disct0
  133.             b3=1
  134.             #获取系数矩阵A
  136.             A.append(b0)
  137.             A.append(b1)
  138.             A.append(b2)
  139.             A.append(b3)
  141.             #计算lso和L矩阵
  142.             L=[]
  143.             rs=disct0#卫星si到测站的几何距离
  144.             psi=C1[ii]#卫星si的伪距观测值
  145.             cdtsi=δt#以米表示的卫星si的钟差
  147.             dtrop=0#对流层延迟改正量,单位米,用简化的 模型计算对流层延迟改正量,单位米,用简化的模型计算
  149.             diono=0#电离层延迟改正量，单位为米，采用无电离层组合观测值时，此处为0
  150.             Drtcm=0#对伪距的差分改正值，此处为0
  152.             lso=psi-rs+cdtsi*c-dtrop-diono+Drtcm
  154.             #将计算后lso添加进L
  155.             L.append(lso)
  156.             xyz.append(X)
  157.             xyz.append(Y)
  158.             xyz.append(Z)
  159.             xyzs.append(xyz)
  161.             A1.append(A)
  162.             L1.append(L)
  164.         #将数组转为矩阵
  165.         A2=mat(A1)
  166.         L2=mat(L1)
  168.         #计算权阵
  169.         mat_P = mat(diag(C2))
  170.         #加入权阵后
  171.         mat_mid = A2.T * mat_P *A2
  173.         ary_mid_tr = around(mat_mid.astype(float), decimals=20)
  175.         mat_mid_tr = mat(ary_mid_tr)
  177.         x2 = mat_mid_tr.I * A2.T * mat_P * L2
  179.         Xi=X0+x2
  181.         if abs(float(x2[0][0]))>0.001 and abs(float(x2[1][0]))>0.001 and abs(float(x2[2][0]))>0.001 :#or abs(float(x2[3][0]))<0.001:
  182.             X0=Xi
  183.             A=[]
  184.             A1=[]
  185.             xyzs=[]
  186.             xyz=[]
  187.             L=[]
  188.             L1=[]
  189.         else:
  190.             X0=Xi
  191.             print("最终的接收机坐标结果为：\n",X0)
  192.             resultx=(abs(-2424425.1013-X0[0,0])/abs(-2424425.1013))*100
  193.             resulty=(abs(5377188.1768-X0[1,0]) /abs(5377188.1768))*100
  194.             resultz=(abs(2418617.7454-X0[2,0])/abs(2418617.7454))*100
  195.             print("误差分析：\n")
  197.             print("X坐标误差为百分之:{}".format(resultx))
  198.             print("Y坐标误差为百分之:{}".format(resulty))
  199.             print("Z坐标误差为百分之:{}".format(resultz))
  200.             break
  201.     return [xyzs,A1,L1,Xi]
  203. def computeE(M,e):
  204.     E = 0.0
  205.     while True:
  206.         E0 = E
  207.         E = M + e*math.sin(E0)
  208.         if abs(E - E0) <0.001:
  209.             break
  210.     return E
  212. def sdgz(Cuc , Cus , Crc , Crs , Cic , Cis , theta):
  213.     g_theta = Cuc * math.cos(2*theta) + Cus * math.sin(2 * theta)
  214.     g_r = Crc * math.cos(2*theta) + Crs * math.sin(2 * theta)
  215.     g_i = Cic * math.cos(2*theta) + Cis * math.sin(2 * theta)
  216.     return g_theta , g_r , g_i
  218. def cal2gps(cal):
  219. # cal2gps 将公历GPS时间转换到GPS周和周内的秒
  220. # 返回列表，周和周内秒
  221.     mjd=cal2mjd(cal)
  222.     #GPS从MJD44244开始
  223.     e=mjd-44244
  224.     week=math.floor(e/7)
  225.     e=e-week*7
  226.     return [week,round(e*86400)]
  228. def cal2mjd(cal):
  229.     # cal2jd 将公历年月日时分秒转换到简化儒略日。
  230.     # 输入公历时间列表，返回儒略日
  231.     if (len(cal) < 6):
  232.         for i in range(len(cal), 6):
  233.             cal.append(0)
  234.     year = cal[0]
  235.     month = cal[1]
  236.     day = cal[2] + (cal[3] * 3600 + cal[4] * 60 + cal[5]) / 86400;
  237.     y = year + 4800
  238.     m = month
  239.     if (year < 0):
  240.         print('Year is wrong')
  241.         return False
  243.     if (m <= 2):
  244.         # 1，2月视为前一年13，14月
  245.         m = m + 12
  246.         y = y - 1
  247.     e = math.floor(30.6 * (m + 1))
  248.     a = math.floor(y / 100)
  249.     # 教皇格雷戈里十三世于1582年2月24日以教皇训令颁布，将1582年10月5日至14抹掉。1582年10月4日过完后第二天是10月15日
  250.     if (year < 1582) or (year == 1582 and month < 10) or (year == 1582 and month == 10 and day < 15):
  251.         b = -38
  252.     else:
  253.         b = math.floor((a / 4) - a)
  254.     c = math.floor(365.25 * y)
  255.     jd = b + c + e + day - 32167.5
  256.     mjd = jd - 2400000.5
  257.     return mjd

3.主函数编写

  1. from data import donfile
  2. from 卫星位置 import nfilecompute
  3. filename="C:\\Users\\Administrator\\Desktop\\GPS!.txt"
  4. nfilecompute(donfile(filename))

三、总结

在程序编写中，没有进行电离层改正等误差改正，没有完善模型，计算误差在米级，误差较小，可以使用，供交流学习。

标签： 算法 人工智能
本文转载自: https://blog.csdn.net/m0_49684834/article/details/128708895
版权归原作者 学测绘的小杨 所有， 如有侵权，请联系我们删除。
登录后发布评论

“【Python】伪距单点定位”的评论:

还没有评论
关于作者
...
overfit同步小助手
文章同步
相关阅读

网络安全法-网络运行安全

使用selenium/drissionpage时如何阻止chrome自动跳转http到https

docker desktop 里部署的Open WebUI 管理员密码忘记了的处理方法

在ubuntu20.04中搭建onsite比赛运行环境

利用开源的低代码表单设计器FcDesigner高效管理和渲染复杂表单结构

Kafka学习笔记

【前端】浏览器输入url到页面呈现发生了什么？