4. SBL定位中的观测权矩阵设计与实现

为什么SBL需要观测权矩阵

短基线定位中，同一时刻各换能器到应答器的斜距并非等精度：声速剖面误差、目标强度、多途、信噪比和入射角都会让不同应答器（或不同换能器—应答器对）的测距标准差不同。等权最小二乘默认所有观测方差相同，会把低质量斜距与高质量斜距同等对待，导致坐标估值被"坏观测"拉偏。观测权矩阵 P 的作用就是把每条斜距的精度差异编码进法方程，使高精度观测获得更大权重。

设观测向量为 l（各斜距），观测矩阵为 A（单位视线方向余弦），权矩阵为对角阵 P = diag(p₁,…,pₙ)，则加权最小二乘解为：

δx = (AᵀPA)⁻¹ AᵀP(l − l₀)
x̂ = x₀ + δx

其中 l₀ 为在当前近似坐标 x₀ 处计算的斜距。权的最常用取法是方差倒数：pᵢ = σ₀²/σᵢ²，σ₀² 为单位权方差（可任取，只影响权值的绝对尺度，不影响解）。若已知各斜距标准差 σᵢ，直接取 pᵢ = 1/σᵢ² 即可。

本节算例的观测数据与精度设定

设船底布设 4 个换能器，海底布设 3 个应答器，共 12 条斜距观测。为突出权矩阵的作用，人为设定各应答器的测距精度差异明显（示例假设值，非实测）：

等权解相当于令所有 pᵢ = 1，即把 σ=0.30 m 的 R3 观测与 σ=0.05 m 的 R1 观测同等看待。加权解则让 R1 的权重是 R3 的 36 倍，从而抑制 R3 的误差影响。

权矩阵设计与加权解算实现

下面给出可直接运行的 Python 实现：构造观测矩阵、按应答器精度生成对角权阵、分别做等权与加权最小二乘，并输出坐标差异。

import numpy as np

# 换能器位置（船体坐标系，示例值，单位 m）
transducers = np.array([
    [ 0.0,  0.0, 0.0],
    [ 2.0,  0.0, 0.0],
    [ 0.0,  2.0, 0.0],
    [ 2.0,  2.0, 0.0],
])

# 应答器近似位置（示例值，单位 m）
beacons = np.array([
    [ 10.0,  5.0, -50.0],   # R1
    [-15.0,  8.0, -55.0],   # R2
    [  5.0,-20.0, -48.0],   # R3
])

# 各应答器测距标准差（示例假设值）
sigma = np.array([0.05, 0.10, 0.30])

# 目标真实位置（用于生成"观测"斜距，示例）
x_true = np.array([1.0, 1.0, -2.0])

# 生成斜距观测：换能器位置 + 目标位置 到 各应答器
obs = []
for b in beacons:
    for t in transducers:
        r = np.linalg.norm(b - (t + x_true))
        obs.append(r)
obs = np.array(obs)

# 观测权：每条斜距按其所属应答器的 sigma 取 1/sigma^2
p_diag = np.repeat(1.0 / sigma**2, 4)   # 每个应答器 4 条观测
P = np.diag(p_diag)

# 近似坐标（解算起点）
x0 = np.array([0.0, 0.0, 0.0])

def build_A_l0(x):
    A, l0 = [], []
    for i, b in enumerate(beacons):
        for t in transducers:
            d = b - (t + x)
            r = np.linalg.norm(d)
            # 视线方向余弦（对目标坐标的偏导）
            A.append(-d / r)
            l0.append(r)
    return np.array(A), np.array(l0)

# 等权解
A, l0 = build_A_l0(x0)
dx_eq = np.linalg.solve(A.T @ A, A.T @ (obs - l0))
x_eq = x0 + dx_eq

# 加权解
dx_w = np.linalg.solve(A.T @ P @ A, A.T @ P @ (obs - l0))
x_w = x0 + dx_w

print("等权解坐标:", x_eq)
print("加权解坐标:", x_w)
print("坐标差异 (加权-等权):", x_w - x_eq)
print("真值:", x_true)

运行后应观察到：加权解更接近真值 x_true，而等权解因受 R3 大误差观测拖累而偏离。坐标差异向量 x_w − x_eq 直观反映权矩阵带来的修正量。注意：本算例的斜距由真值直接生成、未加噪声，因此加权解应几乎精确命中真值；若在 obs 上叠加随机噪声，则加权解的优势会更明显，但结果依赖随机种子，属于示例现象而非固定实测值。

权矩阵对法方程与残差的影响

加权后法方程由 AᵀA 变为 AᵀPA，等价于对每条观测方程先乘以 √pᵢ 再做普通最小二乘。这带来两个可观察的变化：一是低精度观测对应的行被"压缩"，对 δx 的贡献减小；二是加权残差平方和 vᵀPv 才是与权一致的目标函数，评估拟合优度时应使用它而非 vᵀv。

结果判读与验证要点

• 坐标差异方向：加权解相对等权解的修正方向应指向高精度应答器 R1 的几何约束方向，说明权矩阵确实在"听"精度更高的观测。
• 残差检查：分别计算 vᵀv 与 vᵀPv。加权解的 vᵀPv 应小于等权解在同一权阵下的 vᵀPv，这是加权最小二乘的最优性体现。
• 权值敏感性：把 R3 的 σ 从 0.30 改为 0.15 再解算，观察加权解与等权解的差异是否缩小——差异应随精度差距减小而减小，可作为权矩阵设计正确性的自检。
• 病态检查：若某应答器几何构型差（视线方向接近共面），即使权值高也可能使 AᵀPA 条件数变大，需结合几何构型评估，不能只靠调权解决。