1. 课程导入与水下多传感器融合概览

水下环境是典型的“感知困难户”：水体对电磁波衰减极强，GPS 与无线电几乎失效；光视觉在浑浊水中迅速退化；声学通信带宽低、时延大且多径严重。任何单一传感器都难以稳定回答“我在哪、周围有什么、环境怎样”这三个核心问题。水下多传感器融合（Underwater Multi-Sensor Fusion）正是为此而生：它把声学、惯性、视觉、压力、磁力等异构传感器的观测，在时间与空间上对齐，通过估计与推理得到比任何单源都更可靠、更完整的状态描述。

理解融合，先要理解“融合什么”。按信息抽象层次，常见三级划分：数据级融合直接合并原始观测，如多波束测深点云拼接；特征级融合先提取特征再关联，如从声呐图像与相机图像中提取目标轮廓后匹配；决策级融合在各传感器独立给出判断后投票或加权，如多个探测模块分别判定“有目标/无目标”再综合。层级越高，对时间同步与标定精度的要求越低，但信息损失也越大。工程上，导航定位多用特征级与数据级，目标探测常用决策级。

再看“用什么融合”。水下机器人导航的典型组合是 IMU + DVL + 深度计 + 罗经：IMU 提供高频角速度与加速度，但积分漂移快；DVL（多普勒计程仪）给出对底速度，抑制速度漂移；深度计提供绝对深度约束；罗经提供航向。四者在扩展卡尔曼滤波（EKF）框架下互补，形成“高频推算 + 低频绝对校正”的闭环。环境监测则更关注多源环境量的时空一致性，例如把 CTD（温盐深）、溶解氧、浊度传感器的读数与位置轨迹绑定，构建沿航迹的环境剖面。目标探测常采用前视声呐 + 光学相机 + 磁力计的组合，利用声呐的远距离覆盖与光学的近距离高分辨互补。

下图给出本节最值得先建立的整体认知：三类典型应用如何映射到不同的传感器组合与融合层级，以及 Python 在其中的位置。

为什么选 Python？水下融合的难点不在“算得多快”，而在“把异构数据理顺、把算法快速验证”。Python 的优势恰好在此：pyserial、pymodbus 负责与传感器和工控设备通信；numpy、scipy 承担矩阵运算、插值与坐标变换；filterpy 提供现成的卡尔曼滤波实现；gtsam 支持因子图优化；matplotlib、plotly 用于轨迹与误差曲线可视化。真实产品最终可能用 C++ 部署，但算法原型、参数整定与离线回放几乎都在 Python 中完成。

课程的学习路径按“先单源、再多源、后系统”推进。第一阶段建立坐标与时间基准：统一右手坐标系、明确各传感器安装外参、掌握时间戳对齐与插值。第二阶段逐个吃透传感器：IMU 的零偏与积分漂移、DVL 的对底/对水模式差异、深度计的压力换算、罗经的磁偏角修正。第三阶段进入融合：从一维标量卡尔曼滤波讲起，过渡到 EKF 的状态向量设计（位置、速度、姿态、零偏），再到多源异步观测的更新顺序问题。第四阶段做项目实战：用一段模拟或公开的 AUV 航迹数据，完成“IMU 推算 + DVL 校正 + 深度约束”的完整融合，并用均方根误差（RMSE）对比纯 IMU 推算与融合结果。

下面给出一个贯穿课程的最小可运行骨架，帮助建立“输入—处理—输出”的直觉。示例参数为教学假设值，非实测：设 IMU 输出周期 10 ms，DVL 输出周期 200 ms，深度计 100 ms，融合周期取 10 ms，DVL 与深度观测在到达时触发 EKF 更新。

import numpy as np

dt = 0.01                 # 融合周期 10 ms（示例）
x = np.zeros((6, 1))      # 状态：[px, py, vx, vy, yaw, yaw_rate]（示例简化）
P = np.eye(6) * 0.1       # 初始协方差

def predict(x, P, imu):
    # 简化运动模型：匀速 + 航向角速度积分（教学示意）
    yaw = x[4, 0]
    vx, vy = x[2, 0], x[3, 0]
    x[0, 0] += (vx * np.cos(yaw) - vy * np.sin(yaw)) * dt
    x[1, 0] += (vx * np.sin(yaw) + vy * np.cos(yaw)) * dt
    x[4, 0] += imu["yaw_rate"] * dt
    return x, P + np.eye(6) * 0.01

def update_depth(x, P, depth_meas):
    # 以深度作为一维观测，H 选取对应状态分量（此处示意）
    H = np.zeros((1, 6)); H[0, 1] = 1.0
    R = np.array([[0.05]])
    y = depth_meas - H @ x
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ y
    P = (np.eye(6) - K @ H) @ P
    return x, P

运行这段骨架时，值得观察三个量：状态协方差 P 的对角线是否在无观测时持续增大、在有 DVL/深度更新时回落；融合轨迹与纯 IMU 推算轨迹的分离速度；以及残差 y 是否出现持续偏置——若残差长期同号，往往意味着外参或时间戳存在系统性偏差，而不是噪声问题。这正是后续章节要反复训练的“看结果、判原因”的能力。

本课程最终交付的实战项目是一个“小型 AUV 组合导航与目标探测回放系统”：读取预置的多传感器日志，完成时间对齐、EKF 融合、轨迹可视化，并对一段包含疑似目标的声呐数据给出检测结论与置信度。项目不依赖真实下水，但数据格式、时间戳语义与误差评估方式均按工程惯例设计，便于迁移到真实平台。