python 进行卫星坐标计算和接收机坐标计算-Toy模板网

这篇具有很好参考价值的文章主要介绍了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为卫星编号，标蓝的为未知数
$\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$
整理后我们可以得到，接收机概略位置: $X_0,Y_0,Z_0)$ 一开始为0，后续迭代变化直到满足条件
$\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}$
根据以上公式，推到可以得到
$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$
当观测4颗卫星时，j = 1 - 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$
得到矩阵 $ 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