【Python】伪距单点定位
一、算法原理
1.关于伪距单点定位
GPS伪距单点定位的原理比较简单，主要是利用空间距离的后方交会，用一台接收机同时接受四颗卫星的位置坐标和卫星与接收机的距离，运用后方交会原理解算出接收机的三维坐标。其中，如果接收机观测的卫星的数目多于四颗，则采用最小二乘法进行平差计算，求解出接收机坐标。
2.伪距单点定位的具体原理
（1）伪距观测方程
若得到了测站的近似坐标，则可以使用泰勒展开得到线性化方程如下。
（2）列写伪距观测方程，线性化后，得到误差方程：
（3）计算信号发射时刻卫星i的位置与信号到达时接收机的近似距离之间的距离。
3.伪距单点定位程序的计算步骤
1.读取RINEX N文件，将所有星历放置到一个列表ephlst（数组）中。
2.读取RINEX O文件，读取一个历元观测值epoch。
3.数据预处理
根据数组中的卫星号和历元时刻T在ephlst查找相应的卫星星历，
准则
4.程序初始化，设置测站概略位置为Xr，接受机钟差dtr。
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.输出该历元定位结果
二、程序设计
1.读取文件及数据准备
# 数据处理
import numpy
def donfile(path):
with open(path, 'r') as f:
# 获取行数
lines_n = f.readlines()
Ndata = []
# 获取数据组数
data_line_num = int(len(lines_n) / 8)
print(data_line_num)
Ndataitemkeys = ['IODE', 'Crs', 'Delta_n', 'M0', 'Cuc', 'e', 'Cus', 'sqrt_A', 'TEO', 'Cic', 'OMEGA_A0', 'Cis', 'i0',
'Crc',
'omega', 'OMEGA_DOT', 'IDOT', 'L2Codes', 'GPSWEEK', 'L2PCODE', '卫星精度', '卫星健康状态', 'TGD', 'IODC',
'电文发送时刻', '拟合区间', '备用1', '备用2']
# 遍历
for i in range(data_line_num):
Ndataitem = {}
k = 0
for j in range(8):
data_countent = lines_n[8 * i + j]
Ndataitem['数据组号'] = i + 1
if j == 0:
Ndataitem['卫星PRN号'] = data_countent.strip('\n')[0:3]
Ndataitem['历元'] = data_countent.strip('\n')[4:23]
I = data_countent.strip('\n')[23:42] # [:-4]+ 'e' + data_countent.strip('\n')[24:42][-3:]
Ndataitem['卫星钟差参数s'] = str(float(
(data_countent.strip('\n')[23:42][:-4].strip()) + 'e' + data_countent.strip('\n')[23:42][-3:]))
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:])))
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:])))
IB = data_countent.strip('\n')[2:22][-4] # 1-1.731250000000
elif 0 < j < 7:
if data_countent.strip('\n')[4:23][-3] == '-':
Ndataitem[Ndataitemkeys[k]] = str(
float(data_countent.strip('\n')[4:23][:-4].strip()) * numpy.power(10.0, -int(
data_countent.strip('\n')[4:23][-2:])))
else:
Ndataitem[Ndataitemkeys[k]] = str(
float(data_countent.strip('\n')[4:23][:-4].strip()) * numpy.power(10.0,
int(data_countent.strip('\n')[
4:23][-2:])))
if data_countent.strip('\n')[23:42][-3] == '-':
Ndataitem[Ndataitemkeys[k + 1]] = str(
float(data_countent.strip('\n')[23:42][:-4].strip()) * numpy.power(10.0, -int(
data_countent.strip('\n')[23:42][-2:])))
# IB = data_countent.strip('\n')[2:22][-4] # 1-1.731250000000
else:
B = data_countent.strip('\n')[23:42][:-4]
Ndataitem[Ndataitemkeys[k + 1]] = str(
float(data_countent.strip('\n')[23:42][:-4].strip()) * numpy.power(10.0, int(data_countent.strip('\n')[23:42][-2:])))
if data_countent.strip('\n')[42:61][-3] == '-':
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:])))
else:
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:])))
if data_countent.strip('\n')[61:80][-3] == '-':
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:])))
else:
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:])))
k = k + 4
if len(Ndataitem) > 4:
Ndata.append(Ndataitem)
return Ndata
def head_num(lines_n):
for i in range(len(lines_n)):
if lines_n[i].find('END OF HEADER') != -1:
data_num = i + 1
return data_num2.卫星位置以及迭代计算
import math
import numpy
from numpy import *
def nfilecompute(Ndata):
#计算xyz的三维坐标
xyzs = []
A1=[]
L1=[]
#初值测站坐标-2424425.1013 5377188.1768 2418617.7454
#初值化
X0=mat([[0],[0],[0],[0]])
sum=1
#导入C1码数据即伪距观测值
C1=[21937747.840,24275893.980,21322965.800,22097191.620,24075984.740, 21068789.740,20803383.780 ]
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)]
while True:
for ii in range(len(Ndata)):
xyz = []
#系数矩阵
A=[]
c = 299792458 # 光速常数
u = 3.986004418e14#地球引力常数
a = numpy.power(float(Ndata[ii]['sqrt_A']) ,2)#卫星轨道长半径a
n0 = numpy.sqrt(u/numpy.power(a , 3))#参考时刻TOE平均角速度n0
n = n0 + float(Ndata[ii]['Delta_n'])#时刻未定的平均角速度n
e = float(Ndata[ii]['e'])#椭圆轨道偏心率
we = 7.2921151467e-5
cal=[]
cal.append(int(Ndata[ii]['历元'][:4])) #年
cal.append(int(Ndata[ii]['历元'][5:7]))#月
cal.append(int(Ndata[ii]['历元'][9:10]))#日
cal.append(int(Ndata[ii]['历元'][12:13]))#时
cal.append(int(Ndata[ii]['历元'][14:16]))#分
cal.append(int(Ndata[ii]['历元'][17:19]))#秒
#计算参考时刻的时间
t_oc = cal2gps(cal)#将UTC转换为GPS周内秒
#计算观测时间(历元时刻）
t=cal2gps([2022,3,9,0,0])
#卫星钟差改正
a_0=float(Ndata[ii]['卫星钟差参数s'])
a_1=float(Ndata[ii]['卫星钟漂参数s/s'])
a_2=float(Ndata[ii]['卫星钟漂速度参数s/s/s'])
#观测时刻2022,3,9,0,0，计算卫星钟差δt
δt = a_0 + a_1*(t[1] - t_oc[1]) + a_2*((t[1]-t_oc[1])^2)
# dtr = C1[ii] / c #dtr
#接收机钟差
dtr = float(X0[3][0])/c
# print(dtr)
dtsi = δt # 卫星钟差
#while True:
# 选择数据中的一颗卫星的观测值设伪距为C1[ii](需要迭代）
# 计算卫星Sii的信号发射的概略时刻，Tsii
# (a)计算卫星Sii的信号传播时间dtsii
dtsii = C1[ii] / c - dtr + dtsi
Tr = t[1] # Tr历元时刻
# (b)计算卫星Sii的信号发射时刻Tsii
Tsii = Tr - dtsii
t2 = Tsii - δt
#计算dtr
while True:
tk = t2 - float(Ndata[ii]['TEO']) # 规划时间
M = float(Ndata[ii]['M0']) + n*tk#观测卫星瞬间平近点角M TOE参考时刻 t接收机接收卫星信号时刻（变量）
E = computeE(M,e)#利用迭代方式解算偏近点角E
f = 2 * math.atan(numpy.sqrt((1+e)/(1-e))*math.tan(E/2))
_theta = f + float(Ndata[ii]['omega'])
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)
theta = _theta + g_theta
r = a * (1 - e * math.cos(E)) + g_r
i = float(Ndata[ii]['i0']) + g_i + float(Ndata[ii]['IDOT']) * tk
x = r * math.cos(theta)#卫星在轨道平面坐标系中的x
y = r * math.sin(theta)#卫星在轨道平面坐标系中的y
OMEGA = float(Ndata[ii]['OMEGA_A0']) + (float(Ndata[ii]['OMEGA_DOT']) - we)*t[1] - (float(Ndata[ii]['OMEGA_DOT']) * float(Ndata[ii]['TEO']))
#计算地心地固坐标
X1 = x * math.cos(OMEGA) - y * math.cos(i) * math.sin(OMEGA)
Y1 = x * math.sin(OMEGA) + y * math.cos(i) * math.cos(OMEGA)
Z1 = y * math.sin(i)
#初始化，置测站概略位置为xyz_0,接受机钟差初值为dtr
#xyz_0_coord = [0,0,0]
x_0 = float(X0[0][0])
y_0 = float(X0[1][0])
z_0 = float(X0[2][0])
# (c)计算卫星sii在Tsi的位置
#计算发射时刻的卫星坐标(近似坐标)，并且对卫星坐标进行地球自传改正
tch=C1[ii]/c
X=X1*cos(we*tch)+Y1*sin(we*tch)
Y=-sin(we*tch)*X1+Y1*cos(we*tch)
Z=Z1
#计算近似卫星在接受时刻的坐标
#计算卫星和测站的近似几何距离
disct0 = numpy.sqrt(numpy.power((X - x_0), 2) + numpy.power((Y - y_0), 2) + numpy.power((Z- z_0), 2))
#几何距离 求信号传播时间
dts1ii=disct0/c
g=dts1ii-dtsii
if(abs(dts1ii-dtsii)<10.0e-7):
break
else:
dtsii=dts1ii
sum=sum+1
#rs
b0=-(X-x_0)/disct0
b1=-(Y-y_0)/disct0
b2=-(Z-z_0)/disct0
b3=1
#获取系数矩阵A
A.append(b0)
A.append(b1)
A.append(b2)
A.append(b3)
#计算lso和L矩阵
L=[]
rs=disct0#卫星si到测站的几何距离
psi=C1[ii]#卫星si的伪距观测值
cdtsi=δt#以米表示的卫星si的钟差
dtrop=0#对流层延迟改正量,单位米,用简化的 模型计算对流层延迟改正量,单位米,用简化的模型计算
diono=0#电离层延迟改正量，单位为米，采用无电离层组合观测值时，此处为0
Drtcm=0#对伪距的差分改正值，此处为0
lso=psi-rs+cdtsi*c-dtrop-diono+Drtcm
#将计算后lso添加进L
L.append(lso)
xyz.append(X)
xyz.append(Y)
xyz.append(Z)
xyzs.append(xyz)
A1.append(A)
L1.append(L)
#将数组转为矩阵
A2=mat(A1)
L2=mat(L1)
#计算权阵
mat_P = mat(diag(C2))
#加入权阵后
mat_mid = A2.T * mat_P *A2
ary_mid_tr = around(mat_mid.astype(float), decimals=20)
mat_mid_tr = mat(ary_mid_tr)
x2 = mat_mid_tr.I * A2.T * mat_P * L2
Xi=X0+x2
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:
X0=Xi
A=[]
A1=[]
xyzs=[]
xyz=[]
L=[]
L1=[]
else:
X0=Xi
print("最终的接收机坐标结果为：\n",X0)
resultx=(abs(-2424425.1013-X0[0,0])/abs(-2424425.1013))*100
resulty=(abs(5377188.1768-X0[1,0]) /abs(5377188.1768))*100
resultz=(abs(2418617.7454-X0[2,0])/abs(2418617.7454))*100
print("误差分析：\n")
print("X坐标误差为百分之:{}".format(resultx))
print("Y坐标误差为百分之:{}".format(resulty))
print("Z坐标误差为百分之:{}".format(resultz))
break
return [xyzs,A1,L1,Xi]
def computeE(M,e):
E = 0.0
while True:
E0 = E
E = M + e*math.sin(E0)
if abs(E - E0) <0.001:
break
return E
def sdgz(Cuc , Cus , Crc , Crs , Cic , Cis , theta):
g_theta = Cuc * math.cos(2*theta) + Cus * math.sin(2 * theta)
g_r = Crc * math.cos(2*theta) + Crs * math.sin(2 * theta)
g_i = Cic * math.cos(2*theta) + Cis * math.sin(2 * theta)
return g_theta , g_r , g_i
def cal2gps(cal):
# cal2gps 将公历GPS时间转换到GPS周和周内的秒
# 返回列表，周和周内秒
mjd=cal2mjd(cal)
#GPS从MJD44244开始
e=mjd-44244
week=math.floor(e/7)
e=e-week*7
return [week,round(e*86400)]
def cal2mjd(cal):
# cal2jd 将公历年月日时分秒转换到简化儒略日。
# 输入公历时间列表，返回儒略日
if (len(cal) < 6):
for i in range(len(cal), 6):
cal.append(0)
year = cal[0]
month = cal[1]
day = cal[2] + (cal[3] * 3600 + cal[4] * 60 + cal[5]) / 86400;
y = year + 4800
m = month
if (year < 0):
print('Year is wrong')
return False
if (m <= 2):
# 1，2月视为前一年13，14月
m = m + 12
y = y - 1
e = math.floor(30.6 * (m + 1))
a = math.floor(y / 100)
# 教皇格雷戈里十三世于1582年2月24日以教皇训令颁布，将1582年10月5日至14抹掉。1582年10月4日过完后第二天是10月15日
if (year < 1582) or (year == 1582 and month < 10) or (year == 1582 and month == 10 and day < 15):
b = -38
else:
b = math.floor((a / 4) - a)
c = math.floor(365.25 * y)
jd = b + c + e + day - 32167.5
mjd = jd - 2400000.5
return mjd
3.主函数编写
from data import donfile
from 卫星位置 import nfilecompute
filename="C:\\Users\\Administrator\\Desktop\\GPS!.txt"
nfilecompute(donfile(filename))三、总结
在程序编写中，没有进行电离层改正等误差改正，没有完善模型，计算误差在米级，误差较小，可以使用，供交流学习。