使用廣播星曆計算衛星座標(Python)

BIGWangqz發表於2024-08-31
Python

前言

  1. 本程式碼為GNSS課程設計程式碼，僅供參考，使用的計算方法與公式均來源於王堅主編的《衛星定位原理與應用（第二版）》。
  2. 本程式碼計算結果可以透過下載精密星曆進行比照，誤差在1-10m左右。
  3. 實現功能：讀取衛星廣播星曆，並將其計算為WGS-84座標系下的座標，每顆衛星，每15分鐘輸出一次。

廣播星曆下載

  1. 有多重方法進行下載，由於網路原因以及使用的便捷程度，建議使用武漢大學的IGS網站進行下載。（http://www.igs.gnsswhu.cn/index.php）
  2. 檔名不需要填寫，在選擇時間時注意，即使只選擇一天的資料，設定結束時間也要到第二天，否則會顯示錯誤，檢索結果中的數字代表該日期是所選年份的第幾天。

Python函式庫

本程式碼使用numpy，pandas，gnss_lib_py,matplotlib四個函式庫，請提前安裝。

安裝程式碼

#兩條命令根據使用環境進行選擇
pip install gnss-lib-py pandas matplotlib #Python環境安裝程式碼
conda install gnss-lib-py pandas matplotlib -c conda-forge #conda環境安裝程式碼

gnss_lib_py庫

GitHub主頁：https://github.com/Stanford-NavLab/gnss_lib_py?tab=readme-ov-file
文件主頁：https://gnss-lib-py.readthedocs.io/en/latest/index.html
本文主要使用該庫的讀取以及轉化為DataFrame功能，其中引數的命名規則以及時間轉換規則可以在文件中找到。

具體程式碼

建議使用jupyter進行執行，下方程式碼為分單元塊格式，如果沒有jupyter環境可以直接貼上到一個python檔案進行執行。

程式碼塊一：

import gnss_lib_py as glp
import datetime
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

程式碼塊二：

# 匯入23n檔案
file_path = 'brdc2550.23n'
data = glp.RinexNav(file_path)
data_df = data.pandas_df()

程式碼塊三：

# 尋找最小差值的參考時刻
def find(inweekmilli, refers, insv):
    filter_sv = refers[refers['gnss_sv_id'] == insv]
    defference = np.abs(inweekmilli - filter_sv['t_oe'])
    return defference.idxmin()


times = np.array([None] * 24 * 4)
gpsmillis = np.array([None] * 24 * 4)

n = 0
for hour in range(0, 24):
    minut = 0
    while minut < 60:
        times[n] = datetime.datetime(2023, 9, 12, hour, minut, 0, tzinfo=datetime.timezone.utc)
        gpsmillis[n] = glp.datetime_to_gps_millis(times[n])
        minut += 15
        n += 1
gpsmillis = np.array(gpsmillis)

GM=3.986005E+14
sqrtGM = np.sqrt(GM)
sv_list = [f'G{str(i).zfill(2)}' for i in range(1, 33)]
outdata = pd.DataFrame(columns=['data', 'gnss_sv_id', 'X', 'Y', 'Z'], index=range(24 * 4 * 32))
orbit = pd.DataFrame(columns=['data', 'gnss_sv_id', 'x', 'y'], index=range(24 * 4 * 32))
m = 0
j = 0
print("正在計算，請稍候。")
for gpsmilli in gpsmillis:
    for sv in sv_list:
        week, milli_week = glp.gps_millis_to_tow(gpsmilli)
        milli_week=milli_week-18
        index = find(milli_week, data_df, sv)
        print(f'sv:{sv},time:{times[j]},index:{index}')
        print(milli_week,data_df.iloc[index]['t_oe'])
        a = np.power(data_df.iloc[index]['sqrtA'], 2)
        n0 = sqrtGM / np.power(a, 3 / 2)
        n = n0 + data_df.iloc[index]['deltaN']
        tk = milli_week - data_df.iloc[index]['t_oe']
        M = data_df.iloc[index]['M_0'] + n * tk
        e = data_df.iloc[index]['e']
        # 列印中間結果M和e
        print(f"M: {M}, e: {e}")

        # 解開普勒方程
        E = M
        for _ in range(50):  # 使用迭代方法求解E
            E = M + e * np.sin(E)
        
        # 列印中間結果E
        print(f"E: {E}")
        f = np.arctan((np.sqrt(1 - e**2) * np.sin(E)) / (np.cos(E) - e))
        if E > np.pi*0.5:
            f=f+np.pi
        if E < -np.pi*0.5:
            f=f-np.pi
        if np.pi*0.5 > E > 0 > f:
            f=f+np.pi
        if -np.pi*0.5 < E < 0 < f:
            f=f-np.pi
        print(f"arctan({(np.sqrt(1 - e**2) * np.sin(E)) / (np.cos(E) - e)}),f:{f}")
        u_pie = data_df.iloc[index]['omega'] + f
        r_pie = a * (1 - e * np.cos(E))
        C_uc = data_df.iloc[index]['C_uc']
        C_us = data_df.iloc[index]['C_us']
        C_rc = data_df.iloc[index]['C_rc']
        C_rs = data_df.iloc[index]['C_rs']
        C_ic = data_df.iloc[index]['C_ic']
        C_is = data_df.iloc[index]['C_is']
        delta_u = C_uc * np.cos(2 * u_pie) + C_us * np.sin(2 * u_pie)
        delta_r = C_rc * np.cos(2 * u_pie) + C_rs * np.sin(2 * u_pie)
        delta_i = C_ic * np.cos(2 * u_pie) + C_is * np.sin(2 * u_pie)
        u = u_pie + delta_u
        r = r_pie + delta_r
        i = data_df.iloc[index]['i_0'] + delta_i + data_df.iloc[index]['IDOT'] * tk
        print(f'u:{u}')
        x = r * np.cos(u)
        y = r * np.sin(u)
        w_e = 7.292115E-5
        L = data_df.iloc[index]['Omega_0'] + (data_df.iloc[index]['OmegaDot']- w_e )* milli_week - data_df.iloc[index]['OmegaDot']*data_df.iloc[index]['t_oe']
        X = x * np.cos(L) - y * np.cos(i) * np.sin(L)
        Y = x * np.sin(L) + y * np.cos(i) * np.cos(L)
        Z = y * np.sin(i)
        orbit.iloc[m,:] = [times[j],sv,x,y]
        outdata.iloc[m, :] = [times[j], sv, X, Y, Z]
        m += 1
    j += 1
print("由於結果較長，請到Excel中檢視，檔案位於程式碼同級目錄下outdata.csv。")
outdata.to_csv('outdata.csv')
print("匯出成功。")

程式碼塊四：

# 三維座標視覺化顯示
out_sv = 'G20'
fig = plt.figure()
ax = plt.axes(projection='3d')
X = outdata[outdata['gnss_sv_id'] == out_sv]['X']
Y = outdata[outdata['gnss_sv_id'] == out_sv]['Y']
Z = outdata[outdata['gnss_sv_id'] == out_sv]['Z']
ax.plot(X, Y, Z, label=out_sv)
ax.legend()
plt.show()

相關文章