????此程序忽略了實際定位過程中的諸多因素，只用作一個原理的參考！要交作業(yè)的同學請勿復制粘貼，看懂原理才是最重要的，代碼很長包含了很多工具函數(shù)、工具類，熟悉Python的同學可以參考，在此不作講解，只給出源碼和注釋。衛(wèi)星導航定位的原理請自行學習。

????觀測文件(.o)格式：?https://wenku.baidu.com/view/57163341f6335a8102d276a20029bd64783e6231.html 此文章在我編寫程序的過程中起到了重要作用，用以解析觀測文件。精密星歷文件(.sp3)類似可自行查找。

????注意事項：Lagrange插值函數(shù)一定要自己寫！千萬別用SciPy庫里的那個lagrange函數(shù)，否則計算得到的最后結(jié)果與正確理論值誤差能達到五個量級（約十萬米），這個問題困擾了我好幾天，最后發(fā)現(xiàn)是lagrange插值函數(shù)的問題，它計算出來的插值結(jié)果有誤差，在后面的處理中將被放大，是真的拳頭硬了。

????一些基本問題：

1. PC是用線性組合法（具體講是雙頻改正）修正得到的無電離層誤差的信號傳播路徑。

2. 使用Lagrange插值是為了利用精密星歷文件中觀測到的衛(wèi)星精密數(shù)據(jù)（隔一段時間觀測記錄一次，通常是15min）插值得到待求觀測時刻的衛(wèi)星數(shù)據(jù)，包含衛(wèi)星位置、鐘差。

3. 使用最小二乘法是因為觀測方程個數(shù)（等于衛(wèi)星個數(shù)）多于待求解未知數(shù)個數(shù)，可能無法求得同時滿足所有觀測方程的解，因此只能求解一個盡可能靠近所有觀測方程的解。

import numpy as np
#---------------------------------------工具函數(shù)與工具類-------------------------------------------
class LagrangeInterpolationFunc:#Lagrange插值函數(shù)
? ?n=0#插值階數(shù)
? ?x_arr=None
? ?y_arr=None
? ?def __init__(self,x_arr,y_arr):
? ? ? ?if(len(x_arr)!=len(y_arr)):
? ? ? ? ? ?print("Array size is not eq. The larger array will be cut off.")
? ? ? ? ? ?self.n=min(len(x_arr),len(y_arr))-1
? ? ? ?else:
? ? ? ? ? ?self.n=len(x_arr)-1
? ? ? ?self.x_arr=x_arr[0:self.n+1]#截斷數(shù)組，選取較小的size
? ? ? ?self.y_arr=y_arr[0:self.n+1]

? ?def __call__(self,x):
? ? ? ?r=0
? ? ? ?for k in range(0,len(self.x_arr)):
? ? ? ? ? ?lk=1
? ? ? ? ? ?for i in range(0,len(self.x_arr)):
? ? ? ? ? ? ? ?if(i!=k):
? ? ? ? ? ? ? ? ? ?lk=lk*((x-self.x_arr[i])/(self.x_arr[k]-self.x_arr[i]))
? ? ? ? ? ?r=r+lk*self.y_arr[k]
? ? ? ?return r

def str_to_float(str):
? ?if(str.isspace() or len(str)==0):#全是空格表示未知或不存在，取0
? ? ? ?return 0.0
? ?else:
? ? ? ?return float(str.strip())

def str_to_int(str):
? ?if(str.isspace() or len(str)==0):#全是空格表示未知或不存在，取0
? ? ? ?return 0
? ?else:
? ? ? ?return int(str.strip())

def time_to_float(standard_fmt_time):#使用標準時間得到插值橫坐標
? ?h=str_to_float(standard_fmt_time[11:13])
? ?m=str_to_float(standard_fmt_time[14:16])
? ?s=str_to_float(standard_fmt_time[17:])
? ?return h*3600+m*60+s

def time_arr_to_float_arr(standard_fmt_time_arr):#使用標準時間得到插值橫坐標
? ?arr=[]
? ?for t in range(0,len(standard_fmt_time_arr)):
? ? ? ?arr[t]=time_to_float(standard_fmt_time_arr[t])
? ?return arr

def get_interpolation_time(sp3_file,time,forward_num,backward_num):#得到時間time的前forward_num組和后backward_num組星歷的時間
? ?times=[]#記錄時間字符串對應的時間
? ?timesf=[]#記錄時間字符串對應的時間數(shù)字
? ?timef=time_to_float(time)
? ?for t in sp3_file.sp3_data_fields:
? ? ? ?this_time=sp3_file.get(t).time
? ? ? ?times.append(this_time)
? ? ? ?timesf.append(time_to_float(this_time))
? ?times.sort(key=time_to_float)#按時間先后排序
? ?timesf.sort()
? ?start_idx=0
? ?end_idx=-1
? ?for i in range(0,len(timesf)):
? ? ? ?if(timesf[i]>timef):#遍歷時間第一次越過time
? ? ? ? ? ?start_idx=i-forward_num
? ? ? ? ? ?end_idx=i-1+backward_num
? ? ? ? ? ?break
? ? ? ?elif(timesf[i]==timef):
? ? ? ? ? ?start_idx=i-forward_num
? ? ? ? ? ?end_idx=i+backward_num
? ? ? ? ? ?break
? ?return times[start_idx:end_idx+1],timesf[start_idx:end_idx+1]

def standardize_time(time):#將時間格式標準化，把sp3文件記錄的時間格式轉(zhuǎn)換成o文件的格式（空位補零），小數(shù)點精確到后8位，不夠則補零(o文件需要補零)
? ?time=list(time.strip())#字符串轉(zhuǎn)換成列表
? ?if(time[5]==' '):
? ? ? ?time[5]='0'
? ?if(time[8]==' '):
? ? ? ?time[8]='0'
? ?if (time[11]==' '):
? ? ? ?time[11]='0'
? ?if (time[14]==' '):
? ? ? ?time[14]='0'
? ?if (time[17]==' '):
? ? ? ?time[17]='0'
? ?time=''.join(time)
? ?if(len(time)>28):#大于28位則截斷末尾小數(shù)
? ? ? ?time=time[0:28]
? ?elif(len(time)<28):
? ? ? ?time=time.ljust(28,'0')#不夠28位字符則末位補0
? ?return time

def select_satellite(o_satellite_name):#傳入觀測文件中的衛(wèi)星名稱，返回sp3文件的衛(wèi)星名稱
? ?return "PG"+o_satellite_name[1:]

class ODataPack:#16位字符的數(shù)據(jù)包
? ?name=None
? ?data=0
? ?lli=0
? ?ssi=0

? ?def __init__(self,name,data,lli,ssi):#每個觀測數(shù)據(jù)包由數(shù)據(jù)、失鎖標識符LLI、信號強度SSI組成
? ? ? ?self.name=name
? ? ? ?self.data=data
? ? ? ?self.lli=lli
? ? ? ?self.ssi=ssi

? ?def print(self):
? ? ? ?print(self.name," Data:", self.data, " LLI:", self.lli, " SSI:", self.ssi)

class OSatelliteData:#觀測文件中一個衛(wèi)星的數(shù)據(jù)
? ?name=None
? ?data_packs=None#數(shù)據(jù)類型為ODataPack
? ?def __init__(self,name,data_packs=None):
? ? ? ?if(data_packs!=None):
? ? ? ? ? ?self.data_packs=data_packs
? ? ? ?else:
? ? ? ? ? ?self.data_packs={}
? ? ? ?self.name=name

? ?def print(self):
? ? ? ?print("Satellite ", self.name," Total Data ",len(self.data_packs))
? ? ? ?for p in self.data_packs:
? ? ? ? ? ?self.data_packs[p].print()

? ?def add(self,name,data_pack):
? ? ? ?self.data_packs[name]=data_pack

? ?def get(self,name):
? ? ? ?return self.data_packs[name]

class OSatelliteDataField:#一段時間內(nèi)的觀測數(shù)據(jù)
? ?time=None
? ?epoch_mark=0
? ?satellite_num=0
? ?satellite_data=None#數(shù)據(jù)類型為OSatelliteData

? ?def set_info(self,time,epoch_mark,satellite_num):
? ? ? ?self.time=time
? ? ? ?self.epoch_mark=epoch_mark
? ? ? ?self.satellite_num=satellite_num
? ? ? ?self.satellite_data={}

? ?def print(self):
? ? ? ?print("Time:",self.time," Epoch Mark:",self.epoch_mark," Satellite Num:",self.satellite_num)
? ? ? ?for s in self.satellite_data:
? ? ? ? ? ?self.satellite_data[s].print()

? ?def __init__(self,str):
? ? ? ?time=standardize_time(str[0:27])
? ? ? ?epoch_mark=str[27:30].strip()
? ? ? ?satellite_num=str[30:].strip()
? ? ? ?self.set_info(time, epoch_mark, satellite_num)

? ?def add(self,satellite_no,satellite_data):
? ? ? ?self.satellite_data[satellite_no]=satellite_data

? ?def get(self,name):
? ? ? ?return self.satellite_data[name]

? ?def extract_sys_data(self,sys_name):
? ? ? ?sys_data={}
? ? ? ?for s in self.satellite_data:
? ? ? ? ? ?satellite_name=self.satellite_data[s].name
? ? ? ? ? ?if(satellite_name[0]==sys_name):
? ? ? ? ? ? ? ?sys_data[satellite_name]=self.satellite_data[s]
? ? ? ?return sys_data

class OFile:#記錄所有時間段的觀測數(shù)據(jù)
? ?satellite_data_fields=None

? ?def __init__(self):
? ? ? ?self.satellite_data_fields={}

? ?def add(self,time,satellite_data_field):
? ? ? ?self.satellite_data_fields[time]=satellite_data_field

? ?def get(self,time):#以時間為索引
? ? ? ?return self.satellite_data_fields[time]

class SP3File:
? ?sp3_data_fields=None

? ?def __init__(self):
? ? ? ?self.sp3_data_fields={}

? ?def add(self,time,sp3_data_field):
? ? ? ?self.sp3_data_fields[time]=sp3_data_field

? ?def get(self,time): # 以時間為索引
? ? ? ?return self.sp3_data_fields[time]

? ?def print(self):
? ? ? ?for f in self.sp3_data_fields:
? ? ? ? ? ?self.sp3_data_fields[f].print()

class SP3DataField:
? ?time=None
? ?satellite_sp3_data_packs=None

? ?def __init__(self,time):
? ? ? ?self.time=time
? ? ? ?self.satellite_sp3_data_packs={}

? ?def add(self,satellite_no,sp3_data_pack):
? ? ? ?self.satellite_sp3_data_packs[satellite_no]=sp3_data_pack

? ?def add_str(self,str):
? ? ? ?data_pack=SP3DataPack(str)
? ? ? ?self.add(data_pack.name,data_pack)

? ?def get(self,satellite_no): # 以時間為索引
? ? ? ?return self.satellite_sp3_data_packs[satellite_no]

? ?def print(self):
? ? ? ?print("Time ", self.time," Total Data ",len(self.satellite_sp3_data_packs))
? ? ? ?for p in self.satellite_sp3_data_packs:
? ? ? ? ? ?self.satellite_sp3_data_packs[p].print()

class SP3DataPack:
? ?name=None
? ?position_x=0
? ?position_y=0
? ?position_z=0
? ?clock_offset=0
? ?position_x_sd=0
? ?position_y_sd=0
? ?position_z_sd=0
? ?clock_offset_sd=0

? ?def set_info(self,name,position_x,position_y,position_z,clock_offset,position_x_sd,position_y_sd,position_z_sd,clock_offset_sd):
? ? ? ?self.name=name
? ? ? ?self.position_x=position_x
? ? ? ?self.position_y=position_y
? ? ? ?self.position_z=position_z
? ? ? ?self.clock_offset=clock_offset
? ? ? ?self.position_x_sd=position_x_sd
? ? ? ?self.position_y_sd=position_y_sd
? ? ? ?self.position_z_sd=position_z_sd
? ? ? ?self.clock_offset_sd=clock_offset_sd

? ?def __init__(self,str):#直接傳入一行數(shù)據(jù)進行構(gòu)造
? ? ? ?name=str[0:5].strip()
? ? ? ?position_x=str_to_float(str[5:19])#每個數(shù)據(jù)14個字符
? ? ? ?position_y=str_to_float(str[19:33])#每個數(shù)據(jù)14個字符
? ? ? ?position_z=str_to_float(str[33:47])#每個數(shù)據(jù)14個字符
? ? ? ?clock_offset=str_to_float(str[47:61])#每個數(shù)據(jù)14個字符
? ? ? ?position_x_sd=str_to_int(str[61:64])#每個數(shù)據(jù)3個字符
? ? ? ?position_y_sd=str_to_int(str[64:67])#每個數(shù)據(jù)3個字符
? ? ? ?position_z_sd=str_to_int(str[67:70])#每個數(shù)據(jù)3個字符
? ? ? ?clock_offset_sd=str_to_int(str[70:73])#每個數(shù)據(jù)3個字符
? ? ? ?self.set_info(name,position_x,position_y,position_z,clock_offset,position_x_sd,position_y_sd,position_z_sd,clock_offset_sd)

? ?def print(self):
? ? ? ?print("Satellite No. ",self.name," Position X:",self.position_x," Position Y:",self.position_y," Position Z:",self.position_z," Clock Offset:",self.clock_offset," Position X SD:",self.position_x_sd," Position Y SD:",self.position_y_sd," Position Z SD:",self.position_z_sd," Clock Offset SD:",self.clock_offset_sd)

#讀取GPS文件
def read_sp3(path):
? ?time=None
? ?sp3=SP3File()
? ?sp3_data_field=None
? ?for line in open(path):
? ? ? ?if(line[0]=='#' or line[0]=='+' or line[0]=='%' or line.startswith("/*")):
? ? ? ? ? ?continue#注釋行，跳過
? ? ? ?elif(line[0]=='*'):#*開頭的行標注時間
? ? ? ? ? ?if(sp3_data_field!=None):
? ? ? ? ? ? ? ?sp3.add(time,sp3_data_field)
? ? ? ? ? ?time=standardize_time(line[1:])#標準化sp3的時間，以便使用相同的時間進行索引
? ? ? ? ? ?sp3_data_field=SP3DataField(time)
? ? ? ?elif(line.startswith("EOF")):#讀取到EOF標識時結(jié)束
? ? ? ? ? ?sp3.add(time,sp3_data_field)#記錄最后一個時間段的數(shù)據(jù)
? ? ? ? ? ?break
? ? ? ?else:#數(shù)據(jù)行
? ? ? ? ? ?sp3_data_field.add_str(line)
? ?return sp3

#觀測文件格式參考 https://wenku.baidu.com/view/57163341f6335a8102d276a20029bd64783e6231.html
def read_o(path):
? ?data_attribs_text=""
? ?data_attribs={}#數(shù)據(jù)儲存格式
? ?in_data_field=False#是否進入數(shù)據(jù)段
? ?o=OFile()#數(shù)據(jù)數(shù)組
? ?o_data_field=None
? ?for line in open(path):
? ? ? ?if(line[-1]=='\n'):#去掉末尾的換行符
? ? ? ? ? ?line=line[0:-1]
? ? ? ?if(in_data_field):#文件頭結(jié)束進入數(shù)據(jù)段
? ? ? ? ? ?if(line[0]=='>'):#記錄觀測時間、衛(wèi)星個數(shù)的行
? ? ? ? ? ? ? ?if(o_data_field!=None):
? ? ? ? ? ? ? ? ? ?o.add(o_data_field.time,o_data_field)
? ? ? ? ? ? ? ?o_data_field=OSatelliteDataField(line[2:])
? ? ? ? ? ? ? ?continue
? ? ? ? ? ?#處理每一行的數(shù)據(jù)
? ? ? ? ? ?satellite_no=line[0:4].strip()#衛(wèi)星名稱(序號)
? ? ? ? ? ?data_attrib=data_attribs[satellite_no[0]]#獲取該衛(wèi)星所屬系統(tǒng)對應的數(shù)據(jù)格式
? ? ? ? ? ?line=line.ljust(4+len(data_attrib)*16,' ') # 對其字符串，最右邊空位補零，補齊后長度為4+16*n
? ? ? ? ? ?o_data_line=OSatelliteData(satellite_no)
? ? ? ? ? ?for i in range(0,len(data_attrib)):#分割數(shù)據(jù)，每個數(shù)據(jù)占16字符，包含空格
? ? ? ? ? ? ? ?data=str_to_float(line[4+16*i:4+16*(i+1)-3])
? ? ? ? ? ? ? ?lli=str_to_int(line[4+16*(i+1)-3])
? ? ? ? ? ? ? ?ssi=str_to_int(line[4+16*(i+1)-2])
? ? ? ? ? ? ? ?o_data_line.add(data_attrib[i],ODataPack(data_attrib[i],data,lli,ssi))
? ? ? ? ? ?o_data_field.add(satellite_no,o_data_line)#根據(jù)衛(wèi)星序號索引對應行的數(shù)據(jù)
? ? ? ?else:#文件頭
? ? ? ? ? ?if (line.endswith("SYS / # / OBS TYPES")):#數(shù)據(jù)儲存格式
? ? ? ? ? ? ? ?data_attribs_text+=line[0:-21]
? ? ? ? ? ?elif(line.endswith("END OF HEADER")):#文件頭結(jié)束時解析文件頭
? ? ? ? ? ? ? ?in_data_field=True
? ? ? ? ? ? ? ?att_arr=data_attribs_text.split()#解析數(shù)據(jù)儲存格式
? ? ? ? ? ? ? ?for i in range(0,len(att_arr)):#讀取除最后一個系統(tǒng)的其他系統(tǒng)的數(shù)據(jù)格式
? ? ? ? ? ? ? ? ? ?str=att_arr[i]
? ? ? ? ? ? ? ? ? ?if(str.isdigit()):#定位系統(tǒng)數(shù)據(jù)個數(shù)，是一個數(shù)字
? ? ? ? ? ? ? ? ? ? ? ?sys_name=att_arr[i-1]#數(shù)字前一個元素是系統(tǒng)名稱
? ? ? ? ? ? ? ? ? ? ? ?data_attribs[sys_name]=att_arr[i+1:i+1+str_to_int(str)]
? ?return o

#調(diào)試選項
print_o=False#是否打印觀測文件數(shù)據(jù)
print_sp3=False#是否打印精密星歷數(shù)據(jù)
print_A=False#打印A矩陣
print_L=False#打印L向量
print_step_result=False#打印每一次迭代的誤差和結(jié)果
#程序參數(shù)
o_file_path="D:/JFNG1680.20o"
sp3_file_path="D:/igs21102.sp3"
sys_name='G'
time=standardize_time("2020 06 16 10 25 30.0000000")#觀測文件時間
interpolation_forward_num=5#插值時，從目標時間往前的時間段數(shù)
interpolation_backward_num=5#插值時，從目標時間往后的時間段數(shù)
f1=1575.42#MHz
f2=1227.6
position_error_limit=0.0001#位置誤差限制
#---------------------------------------偽距定位主程序-------------------------------------------
#讀取文件和數(shù)據(jù)
o_file=read_o(o_file_path)
o_field=o_file.get(time)#讀取該時間段的觀測數(shù)據(jù)
if(print_o):
? ?print("Obeservation Data")
? ?o_field.print()
? ?print()
o_data=o_field.extract_sys_data(sys_name)#提取該時間段內(nèi)的GPS系統(tǒng)(這里是G)的數(shù)據(jù)
sp3_file=read_sp3(sp3_file_path)#讀取精密星歷文件
#sp3文件九階Lagrange插值使用的時間
interpolation_time,interpolation_timef=get_interpolation_time(sp3_file,time,interpolation_forward_num,interpolation_backward_num)#獲取插值使用的時間，前5組后5組
timef=time_to_float(time)#目標插值時間的數(shù)值
if(print_sp3):
? ?print("SP3 Data")
? ?for t in interpolation_time:
? ? ? ?sp3_file.get(t).print()
? ?print()
it_data_arr={}#索引為衛(wèi)星序號PGxx，值為插值時間段內(nèi)它的各種數(shù)據(jù)數(shù)組
start_field=sp3_file.get(interpolation_time[0])#插值的第一個時間段，此變量用于遍歷獲取衛(wèi)星名稱，因為衛(wèi)星的序號不連續(xù)
for i in start_field.satellite_sp3_data_packs:
? ?satellite_name=start_field.satellite_sp3_data_packs[i].name
? ?x_data_arr=[]
? ?y_data_arr=[]
? ?z_data_arr=[]
? ?co_data_arr=[]
? ?data_arr={}
? ?for j in range(0,len(interpolation_time)):
? ? ? ?sp3_field=sp3_file.get(interpolation_time[j])
? ? ? ?data_pack=sp3_field.get(satellite_name)#獲取PGi的數(shù)據(jù)
? ? ? ?x_data_arr.append(data_pack.position_x*1E3)#km換算成m
? ? ? ?y_data_arr.append(data_pack.position_y*1E3)
? ? ? ?z_data_arr.append(data_pack.position_z*1E3)
? ? ? ?co_data_arr.append(data_pack.clock_offset*1E-6)#μs換算成s
? ?data_arr["Position X"]=x_data_arr
? ?data_arr["Position Y"]=y_data_arr
? ?data_arr["Position Z"]=z_data_arr
? ?data_arr["Clock Offset"]=co_data_arr
? ?it_data_arr[satellite_name]=data_arr
#通過拉格朗日插值法求出2020 06 16 10 25 30.0000000時刻的精密星歷數(shù)據(jù)
sp3_it_function={}#插值的結(jié)果，索引為衛(wèi)星名稱，使用方法同sp3_it_function["PG01"]["Position X"](timef)
for i in it_data_arr:
? ?data_arr=it_data_arr[i]
? ?x_func=LagrangeInterpolationFunc(interpolation_timef,data_arr["Position X"])#Lagrange插值函數(shù)
? ?y_func=LagrangeInterpolationFunc(interpolation_timef,data_arr["Position Y"])
? ?z_func=LagrangeInterpolationFunc(interpolation_timef,data_arr["Position Z"])
? ?co_func=LagrangeInterpolationFunc(interpolation_timef,data_arr["Clock Offset"])
? ?result_arr={}
? ?result_arr["Position X"]=x_func
? ?result_arr["Position Y"]=y_func
? ?result_arr["Position Z"]=z_func
? ?result_arr["Clock Offset"]=co_func
? ?sp3_it_function[i]=result_arr
#-----計算部分-----
n=len(o_data)#觀測衛(wèi)星總個數(shù)
c=299792458#光速m/s
X=np.zeros(shape=(1,4))#未知數(shù)向量初始化為零向量，設置成估計值可以減少迭代次數(shù)。矩陣左上角為(0,0)
L=np.empty(shape=(n,1))
A=np.empty(shape=(n,4))
position_error=float("inf")#位置誤差初始化為無窮大
last_rho={}#上一次的距離ρ
step=0#迭代步數(shù)
if(print_step_result):
? ?print("Initial Value\nPosition X:",X.item(0,0),"Position Y:",X.item(0,1),"Position Z:",X.item(0,2)," Clock Offset:",X.item(0,3))
while position_error>=position_error_limit:
? ?i=0#數(shù)字索引
? ?step=step+1
? ?if(print_step_result or print_L or print_A):
? ? ? ?print("\nStep ",step)
? ?#構(gòu)造觀測方程
? ?for o_satellite_name in o_data:
? ? ? ?sp3_satellite_name=select_satellite(o_satellite_name)
? ? ? ?o_data_pack=o_data[o_satellite_name]
? ? ? ?P1=o_data_pack.get("C1C").data
? ? ? ?P2=o_data_pack.get("C2W").data
? ? ? ?PC=np.square(f1)/(np.square(f1)-np.square(f2))*P1-np.square(f2)/(np.square(f1)-np.square(f2))*P2
? ? ? ?if(len(last_rho)<len(o_data)):#last_rho的元素個數(shù)小于衛(wèi)星個數(shù)則表示這是第一次迭代
? ? ? ? ? ?last_rho[o_satellite_name]=PC#第一次迭代由于未知距離ρ，因此需要單獨修正時間
? ? ? ?modified_timef=timef-last_rho[o_satellite_name]/c#鐘差修正，第一次迭代時ρ沒有值所以修正為PC
? ? ? ?xs=sp3_it_function[sp3_satellite_name]["Position X"](modified_timef)
? ? ? ?ys=sp3_it_function[sp3_satellite_name]["Position Y"](modified_timef)
? ? ? ?zs=sp3_it_function[sp3_satellite_name]["Position Z"](modified_timef)
? ? ? ?dts=sp3_it_function[sp3_satellite_name]["Clock Offset"](modified_timef)
? ? ? ?rho=np.sqrt(np.square(xs-X.item(0,0))+np.square(ys-X.item(0,1))+np.square(zs-X.item(0,2)))
? ? ? ?ax=(xs-X.item(0,0))/rho
? ? ? ?ay=(ys-X.item(0,1))/rho
? ? ? ?az=(zs-X.item(0,2))/rho
? ? ? ?adt=c
? ? ? ?#計算Li和Ai
? ? ? ?Li=PC-rho-c*X.item(0,3)+c*dts
? ? ? ?Ai=np.array([ax,ay,az,adt])
? ? ? ?#將向量分量和矩陣元都裝載進向量L和矩陣A
? ? ? ?L[i][0]=Li
? ? ? ?A[i]=Ai
? ? ? ?i=i+1
? ? ? ?#將此次計算得到的ρ儲存起來用于下一次迭代的插值
? ? ? ?last_rho[o_satellite_name]=rho
? ?if(print_L):
? ? ? ?print("L:\n",L)
? ?if (print_A):
? ? ? ?print("A:\n", A)
? ?#計算dX
? ?Q=np.linalg.inv(np.matmul(A.transpose(),A))#A.shape=(n,4), L.shape=(n,1)
? ?dX=np.matmul(Q,(np.matmul(A.transpose(),L))).transpose()#轉(zhuǎn)置后未知數(shù)向量改正量，shape=(1,4)
? ?position_error=np.sqrt(np.square(dX.item(0,0))+np.square(dX.item(0,1))+np.square(dX.item(0,2)))
? ?X=X-dX#改正X
? ?if(print_step_result):
? ? ? ?print("Position error:", position_error)
? ? ? ?print("Position X:",X.item(0,0),"Position Y:",X.item(0,1),"Position Z:",X.item(0,2)," Clock Offset:",X.item(0,3))
print("\nTotal step",step,", Final Result\nPosition X:",X.item(0,0),"Position Y:",X.item(0,1),"Position Z:",X.item(0,2)," Clock Offset:",X.item(0,3))