import numpy as npimport matplotlib.pyplot as pltfrom mpl_toolkits.mplot3d import Axes3Dfrom scipy.interpolate import interp1dfrom scipy.misc import derivativeimport refrom datetime import datetime, timedeltaimport matplotlib.animation as animation# ===================== 地球常数 =====================GM = 3.986004418e14 # 地球引力常数 m³/s²OMEGA_E = 7.2921151467e-5 # 自转角速度 rad/sEARTH_R = 6378137.0 # 地球半径 m# ===================== 1. RINEX 广播星历解析 =====================def parse_rinex_nav(rinex_file): """解析 RINEX 3.04 导航电文文件，返回卫星PRN对应的星历参数字典""" with open(rinex_file, 'r', encoding='utf-8', errors='ignore') as f: lines = f.readlines() ephemeris = {} prn = None params = [] header_ended = False for line in lines: if "END OF HEADER" in line: header_ended = True continue if not header_ended: continue if re.match(r'[A-Z][0-9]{2}', line[:3]): if prn and len(params) >= 16: ephemeris[prn] = np.array(params[:16], dtype=float) prn = line[:3].strip() params = [] nums = re.findall(r'[-+]?\d*\.\d+[eE][-+]?\d+|[-+]?\d+\.\d+', line) params.extend([float(n) for n in nums]) if prn and len(params) >= 16: ephemeris[prn] = np.array(params[:16], dtype=float) return ephemeris# ===================== 2. 广播星历：位置+速度解算 =====================class BroadcastEphemeris: def __init__(self, params): self.sqrt_a = params[0] self.e = params[1] self.i0 = params[2] self.omega0 = params[3] self.omega = params[4] self.M0 = params[5] self.delta_n = params[6] self.omega_dot = params[7] self.i_dot = params[8] self.Cus, self.Cuc = params[9], params[10] self.Crs, self.Crc = params[11], params[12] self.Cis, self.Cic = params[13], params[14] self.t0 = params[15] def calculate_pos_vel(self, t): tk = t - self.t0 a = self.sqrt_a ** 2 n0 = np.sqrt(GM / a**3) n = n0 + self.delta_n Mk = self.M0 + n * tk Ek = Mk for _ in range(10): Ek = Mk + self.e * np.sin(Ek) sv = np.sqrt(1 - self.e**2) * np.sin(Ek) cv = (np.cos(Ek) - self.e) vk = np.arctan2(sv, cv) phi_k = vk + self.omega du = self.Cus*np.sin(2*phi_k) + self.Cuc*np.cos(2*phi_k) dr = self.Crs*np.sin(2*phi_k) + self.Crc*np.cos(2*phi_k) di = self.Cis*np.sin(2*phi_k) + self.Cic*np.cos(2*phi_k) uk = phi_k + du rk = a*(1 - self.e*np.cos(Ek)) + dr ik = self.i0 + self.i_dot*tk + di omegak = self.omega0 + (self.omega_dot - OMEGA_E)*tk - OMEGA_E*self.t0 x_op = rk * np.cos(uk) y_op = rk * np.sin(uk) x = x_op*np.cos(omegak) - y_op*np.cos(ik)*np.sin(omegak) y = x_op*np.sin(omegak) + y_op*np.cos(ik)*np.cos(omegak) z = x_op*np.sin(ik) + y_op*np.cos(ik) # 速度解算（数值微分） dt = 1e-6 pos_p = self._pos_only(t + dt) pos_m = self._pos_only(t - dt) vx = (pos_p[0] - pos_m[0]) / (2 * dt) vy = (pos_p[1] - pos_m[1]) / (2 * dt) vz = (pos_p[2] - pos_m[2]) / (2 * dt) return np.array([x, y, z]), np.array([vx, vy, vz]) def _pos_only(self, t): return self.calculate_pos_vel(t)[0]# ===================== 3. SP3 精密星历解析 =====================def parse_sp3(sp3_file): times, sat_data = [], {} with open(sp3_file, 'r') as f: for line in f: if line.startswith('*'): continue if line.startswith('P '): parts = line.split() prn = parts[1] x = float(parts[2]) * 1000 y = float(parts[3]) * 1000 z = float(parts[4]) * 1000 if prn not in sat_data: sat_data[prn] = [] sat_data[prn].append([x, y, z]) if line.startswith(' '): try: t = float(line.split()[0]) times.append(t) except: continue t_arr = np.array(times) interpolators = {} for prn, pos in sat_data.items(): pos = np.array(pos) fx = interp1d(t_arr, pos[:,0], kind='cubic', fill_value='extrapolate') fy = interp1d(t_arr, pos[:,1], kind='cubic', fill_value='extrapolate') fz = interp1d(t_arr, pos[:,2], kind='cubic', fill_value='extrapolate') interpolators[prn] = (fx, fy, fz) return interpolators# ===================== 4. 多卫星轨道可视化 =====================class MultiOrbitVisualizer: def __init__(self): plt.rcParams['font.sans-serif'] = ['SimHei'] plt.rcParams['axes.unicode_minus'] = False def plot_3d_multi(self, sat_positions, title="多卫星轨道"): fig = plt.figure(figsize=(12,10)) ax = fig.add_subplot(111, projection='3d') u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j] xe = EARTH_R * np.cos(u)*np.sin(v) ye = EARTH_R * np.sin(u)*np.sin(v) ze = EARTH_R * np.cos(v) ax.plot_surface(xe, ye, ze, color='lightblue', alpha=0.2) colors = ['red', 'green', 'blue', 'orange', 'purple', 'cyan'] for i, (prn, pos) in enumerate(sat_positions.items()): c = colors[i % len(colors)] ax.plot(pos[:,0], pos[:,1], pos[:,2], color=c, linewidth=1.5, label=f'{prn}') ax.scatter(pos[-1,0], pos[-1,1], pos[-1,2], color=c, s=30) ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') ax.set_title(title) ax.legend() plt.show() def animate_3d(self, sat_positions, interval=50): fig = plt.figure(figsize=(12,10)) ax = fig.add_subplot(111, projection='3d') ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') ax.set_title('实时卫星轨道动画') u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j] xe = EARTH_R * np.cos(u)*np.sin(v) ye = EARTH_R * np.sin(u)*np.sin(v) ze = EARTH_R * np.cos(v) ax.plot_surface(xe, ye, ze, color='lightblue', alpha=0.2) colors = ['red','green','blue','orange','purple'] lines = {} points = {} prn_list = list(sat_positions.keys()) for i, prn in enumerate(prn_list): c = colors[i % len(colors)] line, = ax.plot([], [], [], color=c, linewidth=1, label=prn) point, = ax.plot([], [], [], 'o', color=c, markersize=4) lines[prn] = line points[prn] = point max_frames = max([len(sat_positions[p]) for p in prn_list]) ax.legend() def init(): ax.set_xlim(-3e7, 3e7) ax.set_ylim(-3e7, 3e7) ax.set_zlim(-3e7, 3e7) return [] def update(frame): for prn in prn_list: pos = sat_positions[prn] if frame >= len(pos): continue x = pos[:frame, 0] y = pos[:frame, 1] z = pos[:frame, 2] lines[prn].set_data(x, y) lines[prn].set_3d_properties(z) points[prn].set_data([pos[frame,0]], [pos[frame,1]]) points[prn].set_3d_properties(pos[frame,2]) return list(lines.values()) + list(points.values()) ani = animation.FuncAnimation( fig, update, frames=max_frames, init_func=init, interval=interval, blit=True ) plt.show()# ===================== 主程序 =====================if __name__ == "__main__": # ============================================== # 1. 广播星历（RINEX）轨道 + 速度 + 预报 # ============================================== print("正在解析 RINEX 广播星历...") rinex_nav_file = "test.nav" # 替换为你的RINEX文件 try: rinex_data = parse_rinex_nav(rinex_nav_file) t_range = np.linspace(0, 12*3600, 720) sat_orbit = {} for prn, param in rinex_data.items(): eph = BroadcastEphemeris(param) pos_list = [] for t in t_range: pos, vel = eph.calculate_pos_vel(t) pos_list.append(pos) sat_orbit[prn] = np.array(pos_list) viz = MultiOrbitVisualizer() viz.plot_3d_multi(sat_orbit, "RINEX广播星历多卫星轨道（米级）") viz.animate_3d(sat_orbit) except: print("未找到 RINEX 文件，跳过广播星历演示\n") # ============================================== # 2. SP3 精密星历多卫星轨道 # ============================================== print("正在解析 SP3 精密星历...") sp3_file = "test.sp3" # 替换为你的SP3文件 try: sp3_interp = parse_sp3(sp3_file) t_sp3 = np.linspace(0, 12*3600, 720) sp3_orbits = {} for prn, (fx, fy, fz) in sp3_interp.items(): pos = np.vstack([fx(t_sp3), fy(t_sp3), fz(t_sp3)]).T sp3_orbits[prn] = pos viz.plot_3d_multi(sp3_orbits, "SP3精密星历多卫星轨道（厘米级）") viz.animate_3d(sp3_orbits) except: print("未找到 SP3 文件，跳过精密星历演示")
