源码主线 01｜一帧 DVL 怎样成为悬停反馈

这一组课程以项目源码梳理笔记为线索，把函数调用翻译成物理量、公式和可手算的例子。函数与参数对应笔记中的工作区；数值演示与教学流程用于理解机制，不是逐字源码或实机实验结果。先抓住一件事：串口收到速度，并不等于控制器立刻拿到一条可靠的速度反馈。

从字节到有物理意义的观测

函数路线图：Sub::dvl_update → dvl_handle_byte → dvl_data_ALL → dvl_data_BI → DVLVelocityFilter::update → writeBodyFrameOdom → SelectBodyOdomFusion → FuseBodyVel → calcOutputStates → AP_InertialNav_NavEKF::update → poshold_run。前四步解决“这是什么数据”，滤波与 EKF 解决“它怎样约束运动状态”，最后两步解决“控制器需要什么坐标与单位”。

ArduSub/handle_dvl_data.cpp 中的 BI 路径检查整数、分隔符、范围和 A 状态，然后执行安装方向与单位转换。这里不是通用 ENU→NED 公式，而是项目 BI 接口的轴向约定：

$$\text{v\_b = (−v\_x,raw, −v\_y,raw, +v\_z,raw) / 1000 [m/s]}$$

手算一帧：原始 XYZ 为 (−200,100,30) mm/s，转换为 (0.20,−0.10,0.03) m/s，即机体前进、左移并带向下分量。第四个误差量若转换后为 0.011 m/s，则落在 0.002～0.020 m/s 的质量插值区间中点，q=50；质量门限20对应约0.0164 m/s。误差字段、质量分和真实测量方差是三种不同对象。

$$\text{q = 100[1 − clip((|e|−0.002)/0.018, 0, 1)]}$$

为什么有效性需要状态机

ArduSub/dvl_velocity_filter.h 使用 INIT、RECOVERING、VALID、INVALID。恢复至少需要三帧并跨越100 ms，500 ms量级的缺测会使观测失效。想象一次串口积压后同时到达三帧：数量看似足够，但时间覆盖不足；把“帧数”和“持续时间”同时作为恢复条件，就不会把同一瞬间的积压误认成稳定恢复。平滑则用历史窗口降低高频噪声，它同时引入时间上的平均效应。

失效后清除增量时间基准，是为了防止把断流前后两帧直接连成一次巨大积分。恢复首帧先建立基准，第二个可用时刻才形成增量；这与“恢复即把最后速度乘整个断流时间”有本质区别。

为什么传入位移，最终却融合速度

$$\text{Δp\_b = ½(v\_b,k−1 + v\_b,k)Δt； z\_b = Δp\_b / Δt}$$

设两帧滤波后速度分别为 (0.20,−0.10,0.03) 和 (0.24,−0.06,0.01) m/s，间隔0.1 s，梯形积分给出 Δp_b=(0.022,−0.008,0.002) m。EKF 接口再除以0.1 s，得到平均速度观测 (0.22,−0.08,0.02) m/s。Odom 是接口形式，不代表这一路获得了绝对位置。区间平均速度自然对应区间中点，这也解释了传入延迟中为何有半个样本间隔。

观测依次经过 AP_AHRS_NavEKF::writeBodyFrameOdom、NavEKF3::writeBodyFrameOdom 和核心同名函数，写入延迟缓存；SelectBodyOdomFusion 选择可用观测，FuseBodyVel 真正计算创新并修正状态。沿这些函数阅读，比只搜索 DVL 参数更容易看懂传感器和估计器的边界。

把质量、杆臂和创新放进同一公式

$$\text{ẑ\_b = R\_nb v\_n + ω\_b × (r\_DVL−r\_IMU)； ν = ẑ\_b − z\_b}$$

这里 R_nb 把导航系旋转到机体系。第二项是刚体转动产生的传感器点速度；本项目杆臂参数默认约为 (−0.360,0,0.125) m。原地偏航时，偏心 DVL 也会看到线速度，因此它不是简单的“零速时噪声”。观测模型先解释这部分，再用剩余创新修正状态。

$$\text{σ\_v = 0.9 − 0.8(q/100)； R = σ\_v²； S = H P⁻ Hᵀ + R}$$

q=50 时 σ_v=0.5 m/s，R=0.25 (m/s)²；不能把0.5直接填成方差。教学标量例：投影状态方差为0.04，便有S=0.29、K=0.04/0.29≈0.138；预测0.30而观测0.22 m/s时，修正后约0.289 m/s。质量高只是减小R、增加信任，不会强迫状态等于观测。项目的逐轴门限约为ν²/(25S)<1，先判断观测与预测是否相容，再谈增益。

当前默认速度源配置使 body-Z 观测跳过，但倾斜后的 body-X/Y 本身可能包含导航垂直方向的投影。要判断约束了什么，必须看测量雅可比与姿态，而不是把“两个机体轴”直接说成“两个水平轴”。

动手把整条链跑通

离线教学流程：解析帧 → 转轴与单位 → 算q → 更新有效性 → 在连续有效时梯形积分 → 按观测时刻入队 → 计算预测与创新 → 输出当前状态。先只用一维KF复现q=50的算例，再加入偏航杆臂，最后插入500 ms断流。分别画原始速度、滤波速度、融合速度、位置；你会亲眼看到“平滑”“状态估计”“积分位置”不是同一条曲线。

# 教学计算：说明接口中的增量与平均速度，不是项目源码摘录
v_prev = (0.20, -0.10, 0.03)  # m/s
v_now  = (0.24, -0.06, 0.01)
dt = 0.1                    # s
dp = tuple(0.5 * (a + b) * dt for a, b in zip(v_prev, v_now))
z_body = tuple(d / dt for d in dp)
quality = 50
sigma = 0.9 - 0.8 * quality / 100
R = sigma ** 2
# dp=(0.022,-0.008,0.002), z_body=(0.22,-0.08,0.02), R=0.25