2. 多应答器SBL几何构型与观测矩阵组装

几何构型与观测矩阵的对应关系

本节的任务是把“船底换能器阵列 + 多个海底应答器”的空间布局，转换成最小二乘可以直接使用的观测矩阵 A 与观测向量 L。核心认识是：观测矩阵的每一行对应一条“换能器—应答器”斜距观测，行数等于观测总数，列数等于待估参数个数。只要几何构型和观测清单确定，矩阵维度就唯一确定，这也是后续检查数据一致性的第一道关口。

约定坐标系为船体坐标系（右舷为 x 正、船首为 y 正、向上为 z 正），换能器阵列安装在船底，z 取负值；应答器布放于海底，其坐标在船体坐标系下由声学测量或惯导转换得到。设第 i 个换能器位置为 T_i = (x_i, y_i, z_i)，第 j 个应答器位置为 B_j = (X_j, Y_j, Z_j)，则斜距观测为：

r_ij = sqrt( (X_j - x_i)^2 + (Y_j - y_i)^2 + (Z_j - z_i)^2 )

若以应答器坐标为待估参数（多应答器联合标定场景），对 B_j 在概略位置 B_j^0 处线性化，得到观测矩阵中对应行的三个偏导系数：

∂r_ij/∂X_j = (X_j^0 - x_i) / r_ij^0
∂r_ij/∂Y_j = (Y_j^0 - y_i) / r_ij^0
∂r_ij/∂Z_j = (Z_j^0 - z_i) / r_ij^0

这些系数本质上是“从换能器指向应答器”的单位方向余弦，因此观测矩阵的每一行就是一条观测方向在三个坐标轴上的投影。

观测矩阵组装的实现

下面用一段可直接运行的 Python 代码完成本节任务：给定 3 个换能器与 2 个应答器的坐标，计算全部 6 条斜距观测，组装 6×6 观测矩阵与 6×1 观测向量，并做维度与一致性检查。示例坐标均为教学假设值，单位米。

import numpy as np

# 换能器阵列（船底，z 为负）
T = np.array([
    [ 0.0,  0.0, -2.0],   # T1
    [ 1.5,  0.0, -2.0],   # T2
    [ 0.0,  1.5, -2.0],   # T3
])

# 应答器概略坐标（海底）
B0 = np.array([
    [ 20.0,  10.0, -100.0],  # B1
    [-15.0,  25.0, -120.0],  # B2
])

# 模拟“实测”斜距：在真值上叠加小扰动，仅作教学示例
rng = np.random.default_rng(42)
r_true = np.array([[np.linalg.norm(B0[j] - T[i]) for j in range(len(B0))]
                   for i in range(len(T))])
r_obs = r_true + rng.normal(0, 0.02, r_true.shape)

n_obs = T.shape[0] * B0.shape[0]      # 6
n_par = B0.size                       # 6
A = np.zeros((n_obs, n_par))
L = np.zeros(n_obs)

row = 0
for i in range(T.shape[0]):
    for j in range(B0.shape[0]):
        d = B0[j] - T[i]
        r0 = np.linalg.norm(d)
        A[row, 3*j:3*j+3] = d / r0          # 单位方向余弦
        L[row] = r_obs[i, j] - r0
        row += 1

print("A.shape =", A.shape, " L.shape =", L.shape)
assert A.shape == (n_obs, n_par)
assert L.shape == (n_obs,)
print("观测值范围:", r_obs.min().round(3), "~", r_obs.max().round(3))
print("A 每行范数:", np.linalg.norm(A, axis=1).round(6))

运行后应看到 A.shape = (6, 6)、L.shape = (6,)，且 A 每一行的范数都等于 1（因为每行只含一个应答器的三个方向余弦，且该方向向量已归一化）。这是判断观测矩阵组装是否正确的一个快速自检条件：若某行范数明显偏离 1，说明该行系数没有正确归一化，或混入了其他应答器的参数列。

维度与观测值一致性检查

组装完成后，至少要做三项检查，任何一项不通过都不应进入最小二乘解算：

• 行数检查：A 的行数必须等于实际参与解算的斜距观测条数。若某条观测因信噪比过低被剔除，行数应相应减少，而不是保留全零行。
• 列数检查：列数等于待估参数总数，即 3 × 应答器个数。若同时估计换能器安装偏差，则需额外增加列，此时块结构会变为“应答器块 + 换能器块”。
• 观测值一致性：L 中每个元素应等于实测斜距减去同一“换能器—应答器”对在概略坐标下的计算斜距，不能错配对象。可用 r_obs[i,j] - r0 与 L[row] 逐项比对。