源码主线 02｜IMU、深度与延迟输出：同一时刻才可融合

机器人此刻的位置不是三种传感器最新数字的拼接。IMU 描述两次观测之间的运动，DVL校正速度，压力约束垂直位置；它们要在同一时间轴上解释同一个物理状态。这个项目的 UpdateFilter 与 calcOutputStates 正好体现“历史时域融合、当前时域控制”的分工。

IMU 测到的为什么不是位置二阶导数

$$\text{ṗ\_n = v\_n； v̇\_n = R\_bn(f\_b−b\_a)+g\_n； q̇ = ½Ω(ω\_b−b\_g)q}$$

加速度计测的是比力f_b。在NED坐标中，静止且水平时理想加速度计约读(0,0,−g)，旋转到导航系再加g_n才得到零运动加速度。若错误地把读数直接积分，静止机器人也会迅速“下落”。姿态误差同样会把重力投影到水平面：小角度1°造成的虚假水平加速度约g×0.01745=0.171 m/s²，持续1 s就能形成明显速度误差。

外部 IMU10 的固定帧长40字节、帧头AB 04，前39字节做加和校验。AP_InertialSensor_IMU10::publish_latest_sample 将0.01 g转换成m/s²，将0.01 deg/s转换成rad/s，并完成FRD校正。例如原始角速度100表示1°/s，转换为约0.01745 rad/s，不是100 rad/s。read_uart限制单次处理量并发布最新样本，体现了积压处理与控制新鲜度之间的取舍。

三个频率与三个时间不要混为一谈

对象	项目里的具体例子	理解重点
任务调度	IMU/DVL辅助任务200 Hz	每5 ms有机会运行，不等于每5 ms有新观测
传感器发布	IMU后端注册100 Hz	新样本率与调度率是不同层
请求状态机	深度相关更新30 Hz，accumulate 50 Hz	请求、等待、解析可能跨多次调用
采样／接收／融合	设备采样、主机收帧、延迟状态更新	三个时间戳对应不同事件

教学时间线：某DVL平均速度代表9.95 s，串口于10.03 s收齐，EKF在10.05 s处理，控制循环要10.05 s的状态。观测年龄是100 ms；若直接与10.05 s预测比较，在1 m/s²加速度下会制造约0.10 m/s假创新。正确思路是把它放回相应历史状态，再把融合修正传播到当前输出。此例只演示时间关系，不代表项目实际延迟。

延迟缓存不是把所有传感器故意变慢

AP_NavEKF3_Measurements.cpp 建立观测时间；EKF_obs_buffer::recall 取适合融合时域的候选；UpdateFilter 完成预测、协方差传播和观测更新；calcOutputStates 将延迟融合的信息反馈到当前输出预测器。前者追求时间一致，后者追求低控制延迟。控制器因此可以使用当前估计，而不必直接拿一条旧状态闭环。

缓存跨度要覆盖观测延迟，观测本身也要落在可召回窗口内。增大一个“延迟补偿参数”并不自动扩展历史容量。读取这类实现时，应同时追踪样本时间、融合时域和缓存容量，而不是只看一个延迟数值。

压力怎样变成独立深度闭环

$$\text{h =}\frac{{\text{p}}_{\text{water}}\text{−}{\text{p}}_{\text{surface}}}{\text{ρg}}$$

AP_Baro_SerialModbus::update 通过请求／等待／解析状态机获取数据，检查地址、功能码、长度和CRC；AP_Baro再把压力变为高度，readBaroData放入延迟观测缓存。用ρ=1000 kg/m³、g=9.81 m/s²手算，表压19620 Pa对应2 m水深。水密度用1025而不是1000时，同一压力对应约1.951 m；密度属于测量模型，不是PID增益。

control_depth → AC_PosControl::update_z_controller 将深度位置误差变成垂直速度、加速度和推力需求。这里的“独立”是目标与控制回路独立，不是与姿态和分配完全无关：地理垂直需求最终仍要通过完整姿态矩阵投到机体系。横滚较大时，维持深度可能需要 lateral 等通道共同出力。

一份能理解机制的练习

建立一维垂直仿真：高频IMU预测，低频压力修正，再给压力加入100 ms延迟。比较直接融合最新数值与历史状态融合。第二步把ρ调错2%，观察稳定深度偏差；第三步保持真实深度不变而改变姿态，检验旋转后的比力加回重力仍接近零。三个实验分别隔离时间误差、尺度误差与姿态误差。