python 进行卫星坐标计算和接收机坐标计算
python 进行卫星坐标计算和接收机坐标计算
卫星坐标计算
流程以及相关公式
从上一篇文章中我们获取到了广播星历中的文件(N文件读取)，通过N文件中的数据以及周内秒我们可以计算出卫星的坐标。Python读取O文件以及N文件_Hxdih的博客-CSDN博客
在计算卫星坐标时，我们需要做到以下步骤：
- 计算卫星的平均角速度
- 计算卫星的偏近点角
- 计算卫星的真近点角
- 计算卫星的距离
- 计算卫星在轨道平面上的位置坐标（x，y）
- 计算卫星在地心惯性坐标系中的位置 （X，Y，Z ）
根据以上的流程，以及广播星历中的参数我们可以计算出卫星的坐标。
Python代码以及注释
计算周内秒的函数以及公式
# 计算周内秒
def get_week_time(year,month,day,hour,min,second):
weeks = datetime.date(year,month,day).isoweekday()
return (weeks*24+hour)*3600 + min*60 + second
计算卫星坐标
# 定义卫星类
class wx:
def __init__(self,item,t):
# 需要输入卫星星历，和某一时间（周内秒）
self.item = item
self.u = 3.986005e14 # 引力参数
self.a = pow(float(item["sqrt_A"]),2) # a
self.n0 = sqrt(self.u/pow(self.a,3))# n
self.t = t
self.we = 7.292115e-5 # 参数
self.e = self.item['e']
self.X,self.Y,self.Z = self.calculate()
# 确定计算时间
def delta_T(self):
tk = self.t - self.item["TOE"]
return tk
# 平均角速度
def wn(self):
n = self.n0 + self.item["Delta_n"]
return n
# 平近点角M
def pjd_M(self):
M = float(self.item["M0"]) + self.wn() * self.delta_T()
return M
# 偏近点角E
def pjd_E(self):
M = self.pjd_M()
E = 0
while True:
E0 = E
E = M + self.e*sin(E0)
if abs(E-E0) < 0.001:
break
return E
# 真近点角f
def zjd_f(self,E):
f = 2* atan(sqrt((1+self.e)/(1-self.e))*tan(E/2))
return f
# 升角距角θ
def sjjj_ta(self,f,w):
theta = f+float(w)
return theta
# 摄动改正项
def sdgz(self,cuc,cus,crc,crs,cic,cis,theta):
g_t = cuc*cos(2*theta) + cus*sin(2*theta)
g_r = crc*cos(2*theta) + crs*sin(2*theta)
g_i = cic*cos(2*theta) + cis*sin(2*theta)
return g_t,g_r,g_i
# 进行摄动改正
def jxsdgz(self,theta,i0,e,E,idot,g_t,g_r,g_i,a,tk):
theta = theta +g_t
r = a * (1-e*cos(E)) + g_r
i = float(i0)+g_i + float(idot) * tk
return theta,r,i
# 求卫星在轨道平面的坐标
def xy(self,r,theta):
x = r * cos(theta)
y = r * sin(theta)
return x,y
# 计算观测瞬间升交点的经度
def sjdjd(self,omega0,omega,t,toe):
l = float(omega0) + (float(omega) - self.we) * t - (float(omega)*toe)
return l
# 计算卫星瞬时位置
def ss_xy(self,i,L,r,theta):
x,y = self.xy(r,theta)
X1 = x * cos(L) - y * cos(i) * sin(L)
Y1 = x * sin(L) + y * cos(i) * cos(L)
Z1 = y * sin(i)
return X1,Y1,Z1
# 计算最终的卫星坐标
def calculate(self):
item = self.item
tk = self.delta_T()
n = self.wn()
M = self.pjd_M()
Ek = self.pjd_E()
f = self.zjd_f(Ek)
_theta = self.sjjj_ta(f,item["omega"])
g_t,g_r,g_i = self.sdgz(item['Cuc'],item["Cus"],item["Crc"],item["Crs"],item["Cic"],item["Cis"],_theta)
theta,r,i = self.jxsdgz(_theta,item["i0"],self.e,Ek,item["IDOT"],g_t,g_r,g_i,self.a,tk)
l = self.sjdjd(item["OMEGA_A0"],item["OMEGA_DOT"],self.t,item["TOE"])
X,Y,Z = self.ss_xy(i,l,r,theta)
return X,Y,Z
计算接收机坐标
采用的方法是伪距单点定位数学模型
*真实距离 = 卫星到接收机的欧式距离 + c (接收机钟差 - 卫星钟差) + 伪距修正误差 + 电离层误差 + 对流层误差
这里我们先不考虑误差的修正，公式如下：j为卫星编号，标蓝的为未知数
ρ ˉ = ( X j − X ) 2 + ( Y j − Y ) 2 + ( Z j − Z ) 2 + C δ t r − C δ t j \bar{\rho} = \sqrt{(X^j-\textcolor{blue}X)^2 + (Y^j-\textcolor{blue}Y)^2 + (Z^j - \textcolor{blue}Z)^2} + C\textcolor{blue}{\delta t_r} -C\delta t^j ρˉ=(Xj−X)2+(Yj−Y)2+(Zj−Z)2+Cδtr−Cδtj
整理后我们可以得到，接收机概略位置: ( X 0 , Y 0 , Z 0 ) (X_0,Y_0,Z_0) (X0,Y0,Z0) 一开始为0，后续迭代变化直到满足条件
ρ ˉ j ≈ ρ 0 j + ∂ ρ j ∂ X δ X + ∂ ρ j ∂ Y δ Y + ∂ ρ j ∂ Z δ Z + C δ t r − C δ t j ρ 0 j = ( X j − X 0 ) 2 + ( Y j − Y 0 ) 2 + ( Z j − Z 0 ) 2 ∂ ρ j ∂ X = − X j − X 0 ρ 0 j = − l j ∂ ρ j ∂ X = − X j − X 0 ρ 0 j = − m j ∂ ρ j ∂ X = − X j − X 0 ρ 0 j = − n j \begin{align} & \bar{\rho}^j \approx \rho_0^j +\frac{ \partial \rho^j}{\partial X}\delta X + \frac{ \partial \rho^j}{\partial Y}\delta Y+\frac{ \partial \rho^j}{\partial Z}\delta Z + C\delta t_r -C\delta t^j\\ & \rho_0^j = \sqrt{(X^j-X_0)^2 + (Y^j-Y_0)^2 + (Z^j - Z_0)^2} \\ & \frac {\partial \rho ^j} {\partial X} = - \frac {X^j - X_0} {\rho_0^j} = - l^j\\ & \frac {\partial \rho ^j} {\partial X} = - \frac {X^j - X_0} {\rho_0^j} = - m^j\\ & \frac {\partial \rho ^j} {\partial X} = - \frac {X^j - X_0} {\rho_0^j} = - n^j\\ \end{align} ρˉj≈ρ0j+∂X∂ρjδX+∂Y∂ρjδY+∂Z∂ρjδZ+Cδtr−Cδtjρ0j=(Xj−X0)2+(Yj−Y0)2+(Zj−Z0)2∂X∂ρj=−ρ0jXj−X0=−lj∂X∂ρj=−ρ0jXj−X0=−mj∂X∂ρj=−ρ0jXj−X0=−nj
根据以上公式，推到可以得到
l j δ X + m j δ Y + n j δ Z − c δ t r = ρ 0 j − ρ ˉ j − C δ t j l^j \delta X + m^j \delta Y + n^j \delta Z - c \delta t_r = \rho_0^j - \bar{\rho}^j - C \delta t^j ljδX+mjδY+njδZ−cδtr=ρ0j−ρˉj−Cδtj
当观测4颗卫星时，j = 1 - 4 有方程
l 1 δ X + m 1 δ Y + n 1 δ Z − c δ t r = ρ 0 1 − ρ ˉ 1 − C δ t 1 l 2 δ X + m 2 δ Y + n 2 δ Z − c δ t r = ρ 0 2 − ρ ˉ 2 − C δ t 2 l 3 δ X + m 3 δ Y + n 3 δ Z − c δ t r = ρ 0 3 − ρ ˉ 3 − C δ t 3 l 4 δ X + m 4 δ Y + n 4 δ Z − c δ t r = ρ 0 4 − ρ ˉ 4 − C δ t 4 l^1 \delta X + m^1 \delta Y + n^1 \delta Z - c \delta t_r = \rho_0^1 - \bar{\rho}^1 - C \delta t^1\\ l^2 \delta X + m^2 \delta Y + n^2 \delta Z - c \delta t_r = \rho_0^2 - \bar{\rho}^2 - C \delta t^2\\ l^3 \delta X + m^3 \delta Y + n^3 \delta Z - c \delta t_r = \rho_0^3 - \bar{\rho}^3 - C \delta t^3\\ l^4 \delta X + m^4 \delta Y + n^4 \delta Z - c \delta t_r = \rho_0^4 - \bar{\rho}^4 - C \delta t^4 l1δX+m1δY+n1δZ−cδtr=ρ01−ρˉ1−Cδt1l2δX+m2δY+n2δZ−cδtr=ρ02−ρˉ2−Cδt2l3δX+m3δY+n3δZ−cδtr=ρ03−ρˉ3−Cδt3l4δX+m4δY+n4δZ−cδtr=ρ04−ρˉ4−Cδt4
得到矩阵 $ A,X和L$
KaTeX parse error: Undefined control sequence: \pmatrix at position 22: …n{align} & A = \̲p̲m̲a̲t̲r̲i̲x̲{l^1,m^1,n^1,-1…
python代码以及相关注释
# 输入计算完后的卫星坐标字典，对应的O文件数据，初始坐标可以设为（0，0，0）
def compute_xzyz(n_slite,oselect_slite,xyz_1_coord):
# 设置常数 以及 x，y，z
x_0 = xyz_1_coord[0]
y_0 = xyz_1_coord[1]
z_0 = xyz_1_coord[2]
X = np.zeros((4,1))
count_num = 0
c = 299792458
# 无限循环迭代直到满足条件
while True:
count_num += 1
list_A = []
list_L = []
for item in n_slite:
# 卫星坐标
x_j = n_slite[item].X
y_j = n_slite[item].Y
z_j = n_slite[item].Z
# A L 重置
list_A_item = []
list_L_item = []
t = n_slite[item].t # t
toc = n_slite[item].item['TOE'] # toe 卫星星历中参数
# disct0 距离公式 sqrt((x-x0)**2)+(y-y0)**2+(z-z0)**2) # 卫星到用户的欧式距离
disct0 = np.sqrt(np.power((x_j - x_0), 2) + np.power((y_j - y_0), 2) + np.power((z_j - z_0), 2))
# "SMD","DIS","DISV" 卫星钟偏差，卫星钟漂移，卫星钟漂移速度
# 卫星钟差 deltat_r----接收机钟差 deltat_j --- 卫星钟差 --- smd + dis*(t-toe)+disv*(t-toe)**2
deltat_r = float(n_slite[item].item["SMD"]) + float(n_slite[item].item["DIS"]) * (t - toc) + float(n_slite[item].item["DISV"]) * (t - toc) ** 2
# 求导 l,m,n : dx,dy,dz
l = (x_j - x_0) / disct0
m = (y_j - y_0) / disct0
n = (z_j - z_0) / disct0
# A
list_A_item = [l, m, n, -1]
# phi = disct0 - t*c + Is L # 伪距 oselect_slite[item][1]
list_L_item = [disct0 - float(oselect_slite[item][1]) - c * deltat_r + X[3,0]]
# A L
list_A.append(list_A_item)
list_L.append(list_L_item)
# 输出相应参数
# print(f"第{count_num}次循环")
# print(f"当前接收机坐标值：x:{x_0},y:{y_0},z:{z_0}")
# print(item,f"卫星坐标：x:{x_j},y:{y_j},z:{z_j}")
# print(item,f"根据坐标计算：{disct0}")
# print(item,f"计算伪距与观测值之差：{float(oselect_slite[item][2])-disct0}")
# print("当前钟差：",deltat_r)
# print('\n')
# np.mat() 将列表转化成narry格式
A = np.mat(list_A)
L = np.mat(list_L)
# 公式 X = N(-1)U = (A_t A)-1*(At*L) 卫星数量大于4个时使用 当卫星数量=4时 使用 np.linalg.inv(A)*L
X = np.linalg.inv(A.T*A)*(A.T*L)
# 迭代 设置加值
x_0 = X[0, 0] + x_0
y_0 = X[1, 0] + y_0
z_0 = X[2, 0] + z_0
if abs(X[0, 0]) < 0.01 and abs(X[1, 0]) < 0.01 and abs(X[2, 0]) < 0.01 or count_num > 100:
XYZ = [x_0, y_0, z_0, count_num,len(A)]
break
return XYZ
调用使用演示
单秒和逐秒的区别就在于是否迭代字典，以及字典的取值，因此这里只展示逐秒的程序
逐秒计算
# 读取移动站和基准站O N数据
items_ydzO = read_22O(r"XXXXX.23O")
items_ydzN = read_22N(r"XXXXX.23N")
items_jzzO = read_22O(r"XXXXX.23O")
items_jzzN = read_22N(r"XXXXX.23N")
## 装换N文件，方便后续使用
xl_items_jzz = get_needs(items_jzzN)
xl_items_ydz = get_needs(items_ydzN)
# 确定使用的卫星 以及对应的O文件，need 可以为空 或者 为一个卫星编号列表
# 通过这个函数，可以计算出对应卫星的坐标以及后续接收机所需要使用的O文件
def get_data(xl_items,items_22O,d,needs):
Ndata = {}
Odata = {}
for item in xl_items:
if needs != None:
if item in items_22O and item in needs:
temp = xl_items[item]
Ndata[item] = wx(temp,d)
Odata[item] = items_22O[item]
else:
if item in items_22O:
temp = xl_items[item]
Ndata[item] = wx(temp, d)
Odata[item] = items_22O[item]
return Ndata,Odata
if __name__ == '__main__':
# 遍历字典中的数据
for item in items_jzzO:
Ndata_ydz, Odata_ydz = None,None
Ndata_jzz, Odata_jzz = None,None
try:
# 分割时间
temp = item.split()
# 计算当前周内秒
t = abs(get_week_time(int(temp[1]),int(temp[2]),int(temp[3]),float(temp[4]),float(temp[5]),float(temp[6])))
# 计算卫星坐标，以及使用的O文件
Ndata_ydz, Odata_ydz = get_data(xl_items_jzz, items_ydzO[item], t, needs)
Ndata_jzz, Odata_jzz = get_data(xl_items_ydz, items_jzzO[item], t, needs)
except Exception as e:
print(e)
# 计算接收机坐标
if Ndata_jzz != None:
# 初始坐标
xyz_1_coord = [0, 0, 0]
xyz_2_coord = [0, 0, 0]
# 计算移动站坐标
c1 = compute_xzyz(Ndata_ydz, Odata_ydz, xyz_1_coord)
# 伪距修正置0
c1 = compute_xzyz(Ndata_jzz, Odata_jzz, xyz_2_coord)
print(f"移动站修复前：{c1},修复后：{c2}")
jx1 = sqrt(sqrt((c1[0] - c2[0]) ** 2 + (c1[1] - c2[1]) ** 2 + (c1[2] - c2[2]) ** 2)),
print("基线长度:",jx1)
count += 1
结语
通过上篇读取的O文件和N文件数据，我们可以轻松的计算出卫星坐标和接收机坐标，但这个计算过程中忽略了部分误差，导致计算的精度不是非常的高，因此为了提高精度我们需要进行伪距修正等的操作。