136 lines
5.1 KiB
Python
136 lines
5.1 KiB
Python
''' 卫星轨道模型
|
||
根据卫星轨道节点，生成对应的卫星轨道模型。
|
||
整个卫星轨道模型封装成一个单独类。
|
||
'''
|
||
|
||
from os import times
|
||
from time import time
|
||
import numpy as np
|
||
import util
|
||
|
||
def ReconstructionSatelliteOrbit(GPSPoints_list,starttime=1262275200.0):
|
||
'''
|
||
构建卫星轨道
|
||
args:
|
||
GPSPoints_list:卫星轨道点
|
||
starttime:起算时间，默认设置为2010年1月1日，0时0分0秒
|
||
'''
|
||
if len(GPSPoints_list)>16:
|
||
SatelliteOrbitModel=SatelliteOrbit_FitPoly()
|
||
if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSPoints_list,starttime=starttime) is None:
|
||
return None
|
||
return SatelliteOrbitModel
|
||
|
||
class SatelliteOrbit(object):
|
||
def __init__(self) -> None:
|
||
super().__init__()
|
||
self.__starttime=1262275200.0
|
||
|
||
pass
|
||
def get_starttime(self):
|
||
'''
|
||
返回卫星轨道时间起算点
|
||
'''
|
||
return self.__starttime
|
||
def ReconstructionSatelliteOrbit(self,GPSPoints_list):
|
||
'''
|
||
重建卫星轨道，使用多项式拟合法。
|
||
args:
|
||
GPSPoints_list:GPS卫星轨道点。
|
||
return：
|
||
SatelliteOrbitModel 卫星轨道模型
|
||
'''
|
||
self.SatelliteOrbitModel=None
|
||
pass
|
||
|
||
pass
|
||
def SatelliteSpaceState(self,time_float):
|
||
'''
|
||
根据时间戳，返回对应时间的卫星的轨迹状态
|
||
args:
|
||
time_float:时间戳
|
||
return：
|
||
State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
|
||
'''
|
||
return None
|
||
|
||
class SatelliteOrbit_FitPoly(SatelliteOrbit):
|
||
'''
|
||
继承于SatelliteOribit类，为拟合多项式实现方法
|
||
'''
|
||
def __init__(self) -> None:
|
||
super().__init__()
|
||
|
||
def ReconstructionSatelliteOrbit(self,GPSPoints_list,starttime=1262275200.0):
|
||
if len(GPSPoints_list)>16:
|
||
# 多项式的节点数，理论上是超过4个可以起算，这里为了精度选择16个点起算。
|
||
# 多项式 AX=Y ==> A=（X'X)^X'Y，其中 A 为待求系数，X为变量，Y为因变量
|
||
# 这里使用三次项多项式，共有6组参数。
|
||
# 声明自变量，因变量，系数矩阵
|
||
self.__starttime=starttime
|
||
record_count=len(GPSPoints_list)
|
||
time_arr=np.zeros((record_count,1),dtype=np.float64) # 使用np.float64只是为了精度高些，如果32位也能满足需求，请用32
|
||
state_arr=np.zeros((record_count,6),dtype=np.float64)
|
||
A_arr=np.zeros((5,6),dtype=np.float64)
|
||
# 将点记录转换为自变量矩阵、因变量矩阵
|
||
for i in range(record_count):
|
||
GPSPoint=GPSPoints_list[i]
|
||
time_arr[i,0]=GPSPoint[0]-self.__starttime
|
||
state_arr[i,:]=np.array(GPSPoint[1:]).reshape(1,6)
|
||
# 使用最小二乘法，求解系数
|
||
X=np.concatenate([np.ones((record_count,1),dtype=np.float64),time_arr,time_arr**2,time_arr**3,time_arr**4],axis=1)
|
||
|
||
for i in range(6):
|
||
Y=state_arr[:,i]
|
||
A_arr[:,i]=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y).reshape(1,5) #
|
||
self.A_arr=A_arr
|
||
|
||
return A_arr # shape 6x4
|
||
else:
|
||
self.A_arr=None
|
||
return None
|
||
def SatelliteSpaceState(self,time_float):
|
||
'''
|
||
根据时间戳，返回对应时间的卫星的轨迹状态，会自动计算与起算时间之差
|
||
args:
|
||
time_float:时间戳
|
||
return：
|
||
State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
|
||
'''
|
||
if self.A_arr is None:
|
||
return None
|
||
time_float=time_float-self.__starttime
|
||
time_arr=np.ones((1,5),dtype=np.float)
|
||
time_arr[0,0]=time_arr[0,0]
|
||
time_arr[0,1]=time_arr[0,1]*time_float
|
||
time_arr[0,2]=time_arr[0,2]*time_float**2
|
||
time_arr[0,3]=time_arr[0,3]*time_float**3
|
||
time_arr[0,4]=time_arr[0,4]*time_float**4
|
||
SatelliteState_xyz_Vxyz_arr=np.matmul(time_arr,self.A_arr).reshape(1,6)
|
||
return np.concatenate([np.array(time_float+self.__starttime).reshape(1,1),SatelliteState_xyz_Vxyz_arr],axis=1)
|
||
def getSatelliteSpaceState(self,time_array):
|
||
'''
|
||
根据时间戳矩阵，返回对应时刻的卫星空间状态（位置，速度），且会自动计算与起算时间之差
|
||
args:
|
||
time_array:nparray nx1 时间戳
|
||
return:
|
||
SatellitSpaceStateArray:nparray nx6 状态时间
|
||
'''
|
||
if self.A_arr is None:
|
||
return None # 返回None,表示没有结果
|
||
A=np.transpose(self.A_arr,(1,0)) # shape 4x6
|
||
timeRelative1=time_array-self.__starttime # 返回相对值
|
||
n=timeRelative1.shape[0]
|
||
timeRelative=np.ones((n,5))
|
||
timeRelative[:,1]=timeRelative1
|
||
timeRelative[:,2]=timeRelative1**2
|
||
timeRelative[:,3]=timeRelative1**3
|
||
timeRelative[:,4]=timeRelative1**4
|
||
SatelliteSpaceStateArray=np.matmul(timeRelative,A) # nx6
|
||
return SatelliteSpaceStateArray # 位置矩阵
|
||
|
||
pass
|
||
|
||
|
||
|