2. GPS/北斗双系统伪距单点定位

任务目标

实现GPS和北斗双系统伪距单点定位（SPP），利用双系统观测数据联合解算接收机位置与钟差，并评估定位精度。双系统联合可增加可见卫星数，改善单系统几何构型不足时的定位结果。

环境依赖

本案例使用Python实现，依赖numpy进行矩阵运算。卫星位置计算所需广播星历参数采用简化模型（仅使用开普勒轨道根数），适用于教学演示；实际工程中应接入标准RINEX导航文件解析。

核心原理

伪距观测方程为：

ρ = ||r_sat - r_rcv|| + c·(dt_rcv - dt_sat) + ε

其中ρ为伪距观测值，r_sat为卫星位置，r_rcv为接收机位置，c为光速，dt_rcv为接收机钟差，dt_sat为卫星钟差（由广播星历参数计算）。GPS与北斗系统时间基准不同，需分别估计各系统的接收机钟差参数。因此双系统联合定位的未知数为：接收机三维坐标 + GPS接收机钟差 + 北斗接收机钟差，共5个未知数。

关键完整代码

import numpy as np

# ========== 常量 ==========
C = 299792458.0  # 光速 m/s
WGS84_A = 6378137.0
WGS84_F = 1/298.257223563
WGS84_E2 = WGS84_F*(2-WGS84_F)

# ========== 卫星位置计算（简化广播星历） ==========
def sat_position(kepler, t):
    """kepler: [sqrt_a, e, i0, Omega0, omega, M0, delta_n, Omega_dot, toe]
       t: 观测时刻（GPS周内秒）"""
    sqrt_a, e, i0, Omega0, omega, M0, delta_n, Omega_dot, toe = kepler
    a = sqrt_a**2
    n0 = np.sqrt(3986004.418e8/a**3)
    n = n0 + delta_n
    Mk = M0 + n*(t-toe)
    # 开普勒方程迭代
    E = Mk
    for _ in range(10):
        E = Mk + e*np.sin(E)
    # 真近点角
    nu = np.arctan2(np.sqrt(1-e**2)*np.sin(E), np.cos(E)-e)
    phi = nu + omega
    # 轨道平面坐标
    r = a*(1-e*np.cos(E))
    x_orb = r*np.cos(phi)
    y_orb = r*np.sin(phi)
    # 轨道倾角与升交点赤经
    ik = i0
    Omega_k = Omega0 + (Omega_dot-7.2921151467e-5)*(t-toe) - 7.2921151467e-5*toe
    # 地心地固坐标
    x = x_orb*np.cos(Omega_k) - y_orb*np.cos(ik)*np.sin(Omega_k)
    y = x_orb*np.sin(Omega_k) + y_orb*np.cos(ik)*np.cos(Omega_k)
    z = y_orb*np.sin(ik)
    return np.array([x, y, z])

# ========== 卫星钟差 ==========
def sat_clock_bias(af0, af1, t, toe):
    return af0 + af1*(t-toe)

# ========== 最小二乘定位 ==========
def least_squares_spp(obs, sat_pos, sat_clk, sys_idx, x0):
    """
    obs: 伪距观测值 (m)
    sat_pos: 卫星位置 (n,3)
    sat_clk: 卫星钟差 (s)
    sys_idx: 系统标识，0=GPS, 1=BDS
    x0: 初始状态 [x,y,z, dt_gps, dt_bds]
    """
    x = x0.copy()
    for _ in range(10):
        n = len(obs)
        H = np.zeros((n, 5))
        r = np.zeros(n)
        for i in range(n):
            dx = x[0]-sat_pos[i,0]
            dy = x[1]-sat_pos[i,1]
            dz = x[2]-sat_pos[i,2]
            dist = np.sqrt(dx**2+dy**2+dz**2)
            H[i,0] = dx/dist
            H[i,1] = dy/dist
            H[i,2] = dz/dist
            H[i,3] = 1.0 if sys_idx[i]==0 else 0.0
            H[i,4] = 1.0 if sys_idx[i]==1 else 0.0
            r[i] = obs[i] - (dist + C*(x[3+sys_idx[i]] - sat_clk[i]))
        # 最小二乘解
        dX = np.linalg.lstsq(H, r, rcond=None)[0]
        x += dX
        if np.linalg.norm(dX[:3]) < 1e-4:
            break
    return x

# ========== 精度评估 ==========
def ecef2lla(pos):
    x, y, z = pos
    lon = np.arctan2(y, x)
    p = np.sqrt(x**2+y**2)
    lat = np.arctan2(z, p*(1-WGS84_E2))
    for _ in range(5):
        N = WGS84_A/np.sqrt(1-WGS84_E2*np.sin(lat)**2)
        h = p/np.cos(lat) - N
        lat = np.arctan2(z, p*(1-WGS84_E2*N/(N+h)))
    return np.degrees(lat), np.degrees(lon), h

# ========== 示例数据（模拟2颗GPS+2颗北斗卫星） ==========
# 真实接收机位置（ECEF, m）
true_pos = np.array([-2267749.0, 5009354.0, 3221290.0])

# 卫星星历参数（简化：sqrt_a, e, i0, Omega0, omega, M0, delta_n, Omega_dot, toe）
kepler_gps = [
    [5153.6, 0.01, 0.96, -2.5, 0.5, 1.2, 0, 0, 100],
    [5153.7, 0.008, 0.97, -2.4, 0.6, 2.0, 0, 0, 100],
]
kepler_bds = [
    [6496.0, 0.02, 1.05, 1.5, 0.3, 0.8, 0, 0, 100],
    [6496.1, 0.015, 1.04, 1.6, 0.4, 1.5, 0, 0, 100],
]
# 卫星钟差参数 (af0, af1)
clk_gps = [(1e-4, 1e-10), (2e-4, 1e-10)]
clk_bds = [(3e-4, 1e-10), (4e-4, 1e-10)]

# 生成观测时刻与观测值
t_obs = 200.0
sat_pos_list = []
sat_clk_list = []
sys_list = []
obs_list = []

# GPS卫星
for k, c in zip(kepler_gps, clk_gps):
    sp = sat_position(k, t_obs)
    sc = sat_clock_bias(c[0], c[1], t_obs, k[8])
    sat_pos_list.append(sp)
    sat_clk_list.append(sc)
    sys_list.append(0)
    # 生成伪距（含接收机钟差0.1ms，卫星钟差，无噪声）
    rho = np.linalg.norm(sp-true_pos) + C*(0.0001 - sc)
    obs_list.append(rho)

# 北斗卫星
for k, c in zip(kepler_bds, clk_bds):
    sp = sat_position(k, t_obs)
    sc = sat_clock_bias(c[0], c[1], t_obs, k[8])
    sat_pos_list.append(sp)
    sat_clk_list.append(sc)
    sys_list.append(1)
    rho = np.linalg.norm(sp-true_pos) + C*(0.0002 - sc)  # BDS钟差0.2ms
    obs_list.append(rho)

sat_pos = np.array(sat_pos_list)
sat_clk = np.array(sat_clk_list)
sys_idx = np.array(sys_list)
obs = np.array(obs_list)

# ========== 执行定位 ==========
x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
result = least_squares_spp(obs, sat_pos, sat_clk, sys_idx, x0)

# ========== 输出结果 ==========
print("定位结果 (ECEF, m):", result[:3])
print("GPS钟差 (s):", result[3])
print("BDS钟差 (s):", result[4])

# 精度评估
lat, lon, h = ecef2lla(result[:3])
true_lat, true_lon, true_h = ecef2lla(true_pos)
pos_err = np.linalg.norm(result[:3]-true_pos)
print(f"三维位置误差: {pos_err:.2f} m")
print(f"经度误差: {(lon-true_lon)*111320*np.cos(np.radians(true_lat)):.2f} m")
print(f"纬度误差: {(lat-true_lat)*110946:.2f} m")
print(f"高程误差: {h-true_h:.2f} m")

运行方式

python spp_dual_system.py

验证方法

代码中构造了4颗卫星（2颗GPS、2颗北斗）的模拟观测数据，接收机真实位置已知。运行后观察输出：

三维位置误差应接近0（因未加噪声，理论上可收敛到毫米级以下）；
GPS钟差估计值应接近0.0001s，BDS钟差估计值应接近0.0002s；
若将sys_idx全部设为同一系统（如全0），则H矩阵第4列全为0，此时仅能估计4个未知数，验证双系统联合的必要性。

通过条件：三维位置误差小于0.01m，且两个钟差估计值与设定值偏差小于1e-6s，说明双系统联合解算正确。

常见问题

卫星数不足4颗时：单系统至少需4颗卫星才能定位；双系统联合时，若GPS有3颗、BDS有2颗，共5颗卫星即可解算5个未知数，这是双系统联合的核心优势。
系统间时间偏差：GPS与BDS时间基准不同，必须分别估计钟差，否则会引入数米至数十米的定位误差。
实际数据接入：本案例使用简化星历模型，实际应用中需解析RINEX导航文件获取完整广播星历参数，并考虑地球自转改正、相对论效应等误差源。

📚 课程目录