部落客之前準備利用Python編寫精密單點定位程式,奈何寫了一半的讀取檔案代碼,覺得太浪費時間,就此作罷,這些時間不如多用來研究現有代碼,把這部分放棄的代碼拿出來,希望給有想法的小夥伴一些啟迪
代碼雖未完成,但是有一些小函數,可以參考~
# -*- coding:utf-8 -*-
"""
created on MON Jul 5 20:08:25 2021
@author: xymeng
"""
"""
下面為程式資料說明:
本次觀測資料是在UTC時間2021.06.04開始觀測的,對應的GPS周為2160,周内第五天為觀測當天,是以下載下傳igs21605.sp3為精密星曆,
但此檔案隻包含GPS的,若要北鬥的,需要下載下傳WUM0MGXULA_20211560000_01D_05M_ORB.SP3該命名格式的檔案,2021年6月4日年積日為155。
CAS: ftp://ftp.gipp.org.cn/product/dcb/
IGN: ftp://igs.ign.fr//pub/igs/products/mgex/dcb/
CDDIS: ftp://cddis.gsfc.nasa.gov/pub/gps/products/mgex/dcb/
CDDIS:https://cddis.nasa.gov/archive/gnss/products/mgex/2160/ 産品下載下傳目錄
"""
import numpy as np
from numpy import mat
import math as mt
np.set_printoptions(suppress=True) # 取消科學計數法輸出
'''
'''
class Ostruct:
mcorder = 0 #C2I觀測值所處序列号
ncorder = 0 #C6I觀測值所處序列号
ot = [] #觀測值觀測時刻,民用時,年月日時分秒
comoT = [] #GPS時,周,秒
numprn = 0 #某曆元包含prn的個數
oprn = [] #某曆元包含的prn号的清單
odata = []#某曆元下所有觀測值的清單
odatamat = mat(np.ones((60,7))) #觀測值的矩陣
pos = [] #接收機近似位置
class Nstruct:
nt = [] #星曆播發時刻
class Sp3struct:
sp3T = []
sp3data = [] # 該曆元下所有衛星坐标及鐘內插補點
sp3datamat = mat(np.ones((41,4)))
sprn = []
class Read:
'''
ohread函數主要是讀取觀測檔案頭資訊
'''
def ohread(self,path):
ostruct = Ostruct()
i = 0
with open(path,'r+') as ore:
data = ore.readlines()
while i<len(data):
data0 = data[i]
ohdata = data0[60:(len(data0)-1)]
ohdata0 = data0[0:1]
if ohdata == 'APPROX POSITION XYZ':
m = 0
n = 0
positiondata = list(data0.split(' '))
while m<len(positiondata) and n<=2:
if positiondata[m] == '':
m=m+1
else:
x = positiondata[m]
x = float(x)
ostruct.pos.append(x)
n = n+1
m = m+1
if ohdata == 'SYS / # / OBS TYPES' and ohdata0=='C':
p = 0
pcorder = 0
qcorder = 0
q = 0
ctyorder = list(data0.split())
while p < len(ctyorder):
if ctyorder[p] == 'C2I':
pcorder = p-2
p = p + 1
elif ctyorder[q] == 'C6I':
qcorder = q-2
q = q + 1
else:
q = q + 1
p = p + 1
return len(data),pcorder,qcorder
i = i+1
'''
odhread函數主要讀取曆元時刻
'''
def odhread(self,path,i):
with open(path, 'r+') as ore:
data = ore.readlines()
while i < len(data):
ostruct = Ostruct()
data0 = data[i]
oddata = data0[0:1]
if oddata == '>':
otime = data0.split(' ')
year = int(otime[1])
month = int(otime[2])
day = int(otime[3])
hour = int(otime[4])
min = int(otime[5])
sec = float(otime[6])
numprn = otime[9]
ostruct.ot.append(year)
ostruct.ot.append(month)
ostruct.ot.append(day)
ostruct.ot.append(hour)
ostruct.ot.append(min)
ostruct.ot.append(sec)
ostruct.numprn = int(numprn)
return i,data,ostruct.numprn
i = i+1
'''
odataread函數主要用于讀取一個曆元的全部觀測值
'''
def odataread(self,data,i,numprn):
m = 1
while m<=numprn:
ostruct = Ostruct()
odata = data[i + m]
ostruct.odata.append(odata)
m = m+1
if m>numprn:
return ostruct.odata
'''
Sp3numread函數主要用于讀取該sp3中包含的衛星數
'''
def Sp3numread(self,path1):
i = 0
with open(path1, 'r+') as sre:
sp3data = sre.readlines()
while i < len(sp3data):
sp3data0 = sp3data[i]
sp3data1 = sp3data0.split()
if sp3data1[0] == '+':
sp3snum = int(sp3data1[1])
return sp3snum
i = i+1
'''
SP3dhread函數主要讀取曆元時刻
'''
def SP3dhread(self, path1,i):
sp3struct = Sp3struct()
with open(path1, 'r+') as sre:
sp3data = sre.readlines()
while i < len(sp3data):
sp3data0 = sp3data[i]
sp3data1 = sp3data0.split()
if sp3data1[0] == '*':
year = int(sp3data1[1])
month = int(sp3data1[2])
day = int(sp3data1[3])
hour = int(sp3data1[4])
min = int(sp3data1[5])
sec = float(sp3data1[6])
sp3struct.sp3T.append(year)
sp3struct.sp3T.append(month)
sp3struct.sp3T.append(day)
sp3struct.sp3T.append(hour)
sp3struct.sp3T.append(min)
sp3struct.sp3T.append(sec)
return i,sp3data
i = i + 1
'''
Sdataread函數主要用于讀取一個曆元下的全部衛星位置和鐘差資訊
'''
def Sdataread(self,sp3data,i):
sp3struct = Sp3struct()
k = 0
n = 0
while n<150:
sp3data0 = sp3data[i]
sp3data1 =sp3data0.split()
if sp3data1[0][0:2] =='PC':
sp3struct.sp3data.append(sp3data1)
sp3struct.sprn.append(int(sp3data1[0][2:4]))
k = k+1
n = n+1
i = i +1
n = n+1
return sp3struct.sp3data,k,sp3struct.sprn
class Calculate():
def CommonT2GPST(self,commonT):
y,m = 0,0
UT = commonT[3] + commonT[4] / 60 + commonT[5] / 3600 #日内小時數
if commonT[1] <= 2:
y = commonT[0] - 1
m = commonT[1] + 12
else:
y = commonT[0]
m = commonT[1]
JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + commonT[2]+ UT / 24 + 1720981.5 #儒略日
WN = (int)((JD - 2444244.5) / 7) #GPS周, GPS時的起點是1980年1月6日0時(儒略日為2444244.5)
GPSWeek = WN
Wsecond = (JD - 2444244.5 - 7 * WN) * 86400.00 #GPS周内秒
GPSSecond = Wsecond #周内秒的整數部分
tos = Wsecond - GPSSecond #周内秒的不足一秒的部分
return GPSSecond
if __name__ == '__main__':
path = r'C:\Users\23646\Desktop\ppp資料\WUH200CHN_R_20211550000_01D_30S_MO.21o'
path1 = r'C:\Users\23646\Desktop\ppp資料\WUM0MGXULA_20211560000_01D_05M_ORB.SP3'
onum = 1 #讀取觀測值曆元個數計數
sp3num = 1 #讀取精密星曆曆元個數計數
p1 = []
p2 = [] #僞距觀測值清單
C1 = [] #某曆元衛星prn号的清單
sp31 = [] #某曆元下所有衛星prn号的清單
A = mat(np.ones((1,4)))
L = mat(np.ones((1,1)))
read = Read()
ostruct = Ostruct()
sp3struct = Sp3struct()
calculate = Calculate()
k,l,k0,k1,S,C,i1,j1,An= 4,12,0,0,0,0,0,0,0 #k和l為讀取每一行BD的觀測值時用到的循環參數;k0用于向下讀取觀測值行;
# k1用于控制所讀取的行數小于該曆元下的衛星數;S為sp3檔案中北鬥衛星數;C為BD衛星個數;i1和j1是用于控制選取最佳曆元的;An
# 用于向矩陣中增加新列或新行所使用的計數參數
i,pi = 0,0 #目前所處行數
length,p,q = read.ohread(path) #p,q為觀測值矩陣裡面C2I和C6I對應的僞距觀測值的序号
sp3snum = read.Sp3numread(path1) #sp3中共有多少顆衛星
x0,y0,z0 = ostruct.pos[0],ostruct.pos[1],ostruct.pos[2] #接收機初始位置
while i<812:
i,data,n = read.odhread(path,i)
while k1<n :
odata = read.odataread(data, i, n)
obs = odata[k0]
csys = obs[0]
cprn = int(obs[1:3])
obs0 = str(obs[5:len(obs) + 1].split('\t', 27))
if csys =='C':
C1.append(cprn)
C = C + 1
if obs0[2:14] == ' ':
ostruct.odatamat[k1, 0] = 0
else:
ostruct.odatamat[k1, 0] = float(obs0[2:14])
if obs0[14+1*k:14+1*(k+l)] == ' ':
ostruct.odatamat[k1,1] = 0
else:
ostruct.odatamat[k1,1] = float(obs0[14+1*k:14+1*(k+l)])
if obs0[14+2*k+1*l:14+2*(k+l)] == ' ':
ostruct.odatamat[k1,2] = 0
else:
ostruct.odatamat[k1,2] = float(obs0[14+2*k+1*l:14+2*(k+l)])
if obs0[14+3*k+2*l:14+3*(k+l)] == ' ':
ostruct.odatamat[k1,3] = 0
else:
ostruct.odatamat[k1,3] = float(obs0[14+3*k+2*l:14+3*(k+l)])
if obs0[14+4*k+3*l:14+4*(k+l)] == ' ':
ostruct.odatamat[k1, 4] = 0
else:
ostruct.odatamat[k1,4] = float(obs0[14+4*k+3*l:14+4*(k+l)])
if obs0[14+5*k+4*l:14+5*(k+l)] == ' ':
ostruct.odatamat[k1,5] = 0
else:
ostruct.odatamat[k1,5] = float(obs0[14+5*k+4*l:14+5*(k+l)])
if obs0[14+6*k+5*l:14+6*(k+l)] == ' ':
ostruct.odatamat[k1,6] = 0
else:
ostruct.odatamat[k1,6] = float(obs0[14+6*k+5*l:14+6*(k+l)])
if ostruct.odatamat[k1,p] != 1.0 or ostruct.odatamat[k1,q] != 1.0:
p1.append(ostruct.odatamat[k1,p])
p2.append(ostruct.odatamat[k1,q])
k1 = k1 + 1
k0 = k0 + 1
pi0, spadata = read.SP3dhread(path1, pi)
while pi<1:
v = 0
sp3data,S, sp31=read.Sdataread(spadata,pi0)
while v<len(sp3struct.sp3data):
sp3struct.sp3datamat[v,0] = float(sp3struct.sp3data[v][1])
sp3struct.sp3datamat[v,1] = float(sp3struct.sp3data[v][2])
sp3struct.sp3datamat[v,2] = float(sp3struct.sp3data[v][3])
sp3struct.sp3datamat[v,3] = float(sp3struct.sp3data[v][4])
v = v+1
pi = pi+1
pi0 = pi0+sp3snum+1
sp3struct.sp3data.clear()
'''
以下代碼塊是尋找最佳曆元,并建構矩陣
'''
ot = calculate.CommonT2GPST(ostruct.ot)
spt = calculate.CommonT2GPST(sp3struct.sp3T)
if abs(ot-spt)<=150:
while i1<len(C1):
if C1[i1] == sp31[j1] and j1<len(sp31)-1:
m1 = j1
if p1[i1] != 0:
X1 = sp3struct.sp3datamat[m1,0]
Y1 = sp3struct.sp3datamat[m1,1]
Z1 = sp3struct.sp3datamat[m1,2]
deltat = sp3struct.sp3datamat[m1,3]
Distance = mt.sqrt(((x0 - X1)**2 + (y0-Y1)**2 + (z0-Z1)**2))
li = (X1-x0)/Distance
mi = (Y1-y0)/Distance
ni = (Z1-z0)/Distance
if An == 0:
A[0,0] = li
A[0,1] = mi
A[0,2] = ni
A[0,3] = -1
An = An +1
else:
an = [li,mi,ni,-1]
A = np.row_stack((A, an))
i1 = i1 + 1
j1 = 0
else:
i1 = i1 +1
j1 = 0
else:
if j1 == len(sp31)-1:
i1 = i1 + 1
j1 = 0
else:
j1 = j1 + 1
'''
以下是觀測值一個曆元處理結束後的對清單及矩陣的清空及處理工作
'''
k1 = 0
k0 = 0
C = 0
odata = []
C1.clear()
sp31.clear()
ostruct.odata.clear()
ostruct.ot.clear()
p1.clear()
p2.clear()
ostruct.odatamat = mat(np.ones((60, 7)))
i = i + n + 1