02.4｜DVL不是绝对位置：体轴观测、杆臂、质量与延迟融合

输入是经过安装方向转换的DVL体轴速度、角速度、杆臂、质量和代表时刻；输出是同一时刻对15维误差状态的约束。假设使用稳定底跟踪，海底在导航系近似静止；若设备输出水跟踪速度，观测的是相对水体速度，模型必须增加海流速度，不能当成地速。还假设杆臂已标定且刚体不变，DVL坐标已变换到b系。

符号／对象	含义与维度	单位与坐标
$z_b,h_b(x)$	DVL观测及预测；3维	m/s，机体FRD
$\ell^b$	IMU参考点指向DVL测点的杆臂	m，机体系；方向不能反写
$\hat\omega,u_b$	去偏角速度、 $\hat R^T\hat v$	rad/s、m/s，机体系
$H_b,C$	全轴导数、轴选择矩阵	3×15、m×3，m是选用轴数
$q,\sigma_v,R_b$	质量分、标准差、全轴观测协方差	0—100无量纲、m/s、m²/s²
$r,S,\gamma$	创新、创新协方差、归一化创新平方	m/s、m²/s²、无量纲

需要补的数学只读这些： 叉乘与矩阵乘法和局部斜率。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

看公式之前：先问“谁的速度、哪个方向、哪个时刻”

站在旋转门轴心的人可以不移动，门边的人却在走圆弧。DVL离IMU有杆臂时也是如此：DVL测点速度不等于IMU参考点速度。后面的叉乘不是额外滤波技巧，而是在回答这个刚体几何问题。将量转到同一坐标系只是第一步，还必须把参考点和时间对齐。

三种操作不要互相替代： 杆臂模型把测点运动解释清楚；雅可比描述该模型对小状态误差的敏感性；时间缓存寻找读数真正对应的历史状态。增大 $R$ 可以承认读数更不确定，却不能从原理上修复反向杆臂或错时比较。

以匀加速直线运动作时间检查：加速度 $a=0.5\,\mathrm{m/s^2}$ ，读数比当前状态早 $\Delta t=0.1\,\mathrm s$ 。即使传感器完全准确，旧读数减当前速度也会产生约 $-a\Delta t=-0.05\,\mathrm{m/s}$ 的创新。负创新在这里不是“预测速度偏大”的证据，而是比较了不同时间；静止测试可能完全看不出这个问题。

时间先后与信息先后是两回事

“刚收到”说明消息新到达，不说明物理样本新产生。延迟更新应先定位采样时刻，用那个时刻的状态、协方差和观测模型计算创新；随后通过一致的历史重放或延迟融合／输出预测架构影响当前输出。历史观测不能直接与当前预测相减，再只靠一个更大的门限放行。

微练习： 原地转动时DVL横向读数非零，是否必须把机器人位置向侧面修正？又该怎样区分杆臂问题和时间问题？

概念答案：先解释可预测的运动

不必须。先由角速度与杆臂计算测点速度，再形成创新；模型解释得了的读数不应被当成参考点平移。固定转速、换转向可检查杆臂符号；改变加速度与延迟、比较采样时刻可暴露错时误差。做延迟缓存练习先确保查找和插值正确，但它不代替完整滤波回滚。

先推杆臂速度，再求观测雅可比

1．测点偏离IMU时，原地转动也会有线速度

DVL测的是自身声学测量参考点的速度，而惯性状态通常以IMU参考点为基准。记 $p_D^n$ 为DVL点位置， $p_I^n$ 为IMU点位置， $\ell^b$ 为“IMU指向DVL”的固定杆臂，单位m，机体系表达。刚体几何先给出

p_D^n=p_I^n+R_b^n\ell^b.

对时间求导，杆臂在机体系不变，所以 $\dot\ell^b=0$ ；利用 $\dot R_b^n=R_b^n[\omega^b]_\times$ ，得到

v_D^n=v_I^n+R_b^n(\omega^b\times\ell^b).

左乘 $(R_b^n)^T$ 转到机体系，并以去偏陀螺角速度预测转动项，便是实际需要的观测函数：

h_b(x)=(R_b^n)^Tv_I^n+(\omega_m^b-b_g)\times\ell^b,\qquad z_b=h_b(x)+\epsilon_b.

量	角色	单位／坐标
$z_b,h_b$	DVL读数及同一物理量的预测	$\mathrm{m/s}$ ，机体FRD
$(R_b^n)^Tv_I^n$	参考点平移速度在体轴的投影	$\mathrm{m/s}$ ，b系
$(\omega_m^b-b_g)\times\ell^b$	测点因刚体转动产生的速度	$\mathrm{m/s}$ ，b系
$\epsilon_b$	其余观测误差	$\mathrm{m/s}$ ，b系

手算原地偏航： $v_I=0$ 、 $\omega=[0,0,0.2]^T\ \mathrm{rad/s}$ 、 $\ell=[-0.360,0,0.125]^T\ \mathrm m$ ，叉乘结果为 $[0,-0.072,0]^T\ \mathrm{m/s}$ 。这应由预测函数解释，而不是被当作机器人参考点左移。如果杆臂方向写反，补偿不仅不能消除误差，反而会把它加倍。

2．对DVL观测求导：每一列都表示一个假想小扰动

增益需要 $H$ 。求 $H$ 的意思是“只让某个状态小变一点，DVL预测会怎样变”。令 $u_b=\hat R^T\hat v$ ，其单位为m/s。位置不直接进入瞬时速度观测，因此位置三列为零；加速度偏置也没有直接进入当前观测，所以对应三列为零。

速度扰动显然产生 $\hat R^T\delta v$ 。姿态不能直接对欧拉角求导：使用右扰动 $R_t=\hat R\operatorname{Exp}([\delta\theta]_\times)$ ，则

R_t^Tv_t\approx(I-[\delta\theta]_\times)\hat R^T(\hat v+\delta v) \approx u_b+\hat R^T\delta v-[\delta\theta]_\times u_b.

舍去两误差相乘后，用 $-[\delta\theta]_\times u_b=[u_b]_\times\delta\theta$ ，得到姿态块的正号。陀螺偏置扰动让角速度变化为 $-\delta b_g$ ，因此杆臂速度变化为 $-\delta b_g\times\ell=[\ell]_\times\delta b_g$ ，又是一个正号。合起来：

\delta h_b=\hat R^T\delta v+[u_b]_\times\delta\theta+[\ell^b]_\times\delta b_g.

按15维误差排序读取系数，得到

H_b=\begin{bmatrix}0&\hat R^T&[u_b]_\times&0&[\ell^b]_\times\end{bmatrix}.

这是 $3\times15$ 。例如 $\hat R=I$ 、 $\hat v=[0.4,0,0]^T$ ，姿态偏航小误差 $\delta\theta_z=0.01$ 造成预测体轴右向速度变化 $-0.004\ \mathrm{m/s}$ 。这是机器人朝向轻微变化后，同一导航速度在体轴上出现了横向分量，并不是新增了真实侧向加速度。

如果仅融合部分机体轴，定义轴选择矩阵 $C\in\mathbb R^{m\times3}$ 。选择 $X$ 、Y时 $C=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix}$ 。均值、导数和协方差必须一起投影：

z=Cz_b,\quad h=Ch_b,\quad H=CH_b,\quad R_{\mathrm{obs}}=CR_bC^T.

最后才形成创新与创新协方差： $r=z-h(\hat x)$ 、 $S=HP^-H^T+R_{\mathrm{obs}}$ 。不要只删掉观测值却保留旧 $H$ 或旧 $R$ 的维度。若机器人倾斜，body- $X$ /Y观测也可能含导航系垂直速度，因此“只融两个体轴”与“只约束导航水平”不是一回事。

$H$ 有3行15列；速度块把导航系速度误差投到机体系，姿态块的单位是m/s每rad，陀螺偏置块是m每单位角速率。加速度偏置虽不直接进入 $H$ ，仍可借 $P$ 的互协方差被修正。如果用同一个陀螺样本补偿杆臂，该样本的噪声也会进入观测误差，并可能与传播噪声相关；入门实现通常在小杆臂、小陀螺噪声下忽略相关性，更精细模型需联合处理，不能把所有噪声无条件宣称独立。

手算：机器人参考点静止，偏航角速率(0,0,0.2) rad/s，杆臂(−0.360,0,0.125) m。叉乘得到(0,−0.072,0) m/s。若DVL读到这个速度，扣除模型后创新应近零；若漏掉杆臂，滤波器会误以为机器人正在向左平移。轴选择也有几何含义：项目默认配置跳过body-Z，而倾斜时body- $X$ /Y仍含导航垂直速度投影，不能称其为严格的导航水平观测。

质量不是概率，门限也不是滤波增益

3．质量字段怎样进入R，创新门限又回答另一个问题

质量分首先回答“设备自己认为这帧有多可靠”。项目BI误差字段记为 $e$ ，转换后单位m/s；映射的端点是 $0.002$ 和 $0.020\ \mathrm{m/s}$ 。把它归一化到0—1区间，截断后反向变成0—100分：

q=100\left[1-\operatorname{clip}\left(\frac{|e|-0.002}{0.018},0,1\right)\right].

$\operatorname{clip}(x,0,1)$ 只是区间截断，不是概率函数。取 $e=0.011$ ，归一化值为0.5，所以 $q=50$ 。项目再用质量决定速度标准差：

\sigma_v=0.9-0.8\frac q{100},\qquad R_b=\sigma_v^2I.

$\sigma_v$ 单位m/s， $R_b$ 单位 $\mathrm{m^2/s^2}$ ；各向同性写法表示三轴先采用相同标准差。 $q=50$ 因而给 $\sigma_v=0.5$ 、 $R_b=0.25I$ 。质量低意味着减小这条观测的权重；它本身没有检查“这帧是否与当前运动预测一致”。后一个问题由创新统计量回答。

若同时检验m维观测，使用

\gamma=r^TS^{-1}r.

这是由 $S$ 白化后的创新平方和，单位消掉了。单轴检查则用 $\gamma_i=r_i^2/S_{ii}$ 。它衡量的不是绝对误差大小，而是“相对于当前可预期不确定性，差异是否过大”。例如 $r=-0.08$ 、 $S=0.29$ ，有 $\gamma_i=0.02207$ ；同一个创新若 $S$ 只有 $10^{-4}$ ，统计量会变成64。

项目逐轴门限写为 $r_i^2/(25S_{ii})<1$ ，即 $\gamma_i<25$ 。三轴联合检验不能机械套这个阈值，因为维数改变后统计分布不同。门限回答是否接纳， $K$ 回答接纳后怎样加权；两者职责不要合并。

流程上先排除非法状态、丢底锁定、严重异常和陈旧观测，再用统计门限判断预测与测量是否相容；通过后才计算或应用修正。质量低可以增大 $R$ ，硬无效不能只靠一个巨大 $R$ 掩盖。连续低通或滑窗会让相邻DVL输出相关，若仍按独立白噪声重复融合，应适当降低融合频率或建模相关性；预滤波“更平滑”不自动等于更多信息。

BodyFrameOdom接口为何传位移却融合速度

4．BodyFrameOdom为何叫里程计，却并没有得到绝对位置

假设上一帧和当前帧体轴速度分别为 $v_{b,k-1}$ 、 $v_{b,k}$ ，间隔 $\Delta t$ 。若短区间内速度近似线性变化，用平均速度乘时间获得位移增量：

\Delta p_b=\frac{v_{b,k-1}+v_{b,k}}2\Delta t.

$\Delta p_b$ 单位m，描述这段时间的运动增量；它不是地理坐标中的绝对位置。接口收到位移增量与区间长度后，恢复的是区间平均速度：

z_b=\frac{\Delta p_b}{\Delta t}=\frac{v_{b,k-1}+v_{b,k}}2.

前后两次速度为 $[0.20,-0.10,0.03]^T$ 和 $[0.24,-0.06,0.01]^T\ \mathrm{m/s}$ ，时间为0.1 s，先得到位移 $[0.022,-0.008,0.002]^T\ \mathrm m$ ，再除以时间得到 $[0.22,-0.08,0.02]^T\ \mathrm{m/s}$ 。写出这两步，就能理解为什么不能把它送进绝对位置观测方程 $h(x)=p$ 。

三点实现细节：第一，区间必须连续有效，断流前后的帧不能拼成一个长区间；第二，观测时间应该描述这个区间，而不是接收时刻；第三，若区间内机体转动明显，两个速度向量属于不同瞬时体轴，简单逐分量平均只是短区间近似。后面的上游源码将直接展示 $\texttt{delPos}/\texttt{delTime}$ 这一步。

项目路线为 Sub::dvl_update → dvl_handle_byte → dvl_data_ALL → dvl_data_BI → DVLVelocityFilter::update → writeBodyFrameOdom → SelectBodyOdomFusion → FuseBodyVel。两帧(0.20,−0.10,0.03)、(0.24,−0.06,0.01) m/s，间隔0.1 s，产生(0.022,−0.008,0.002) m增量，接口回算为(0.22,−0.08,0.02) m/s平均速度。因此“Odom”不意味着这一路具备绝对位置。跨区间体轴变化较大时还应处理旋转，简单逐分量梯形积分只是短区间近似。

DVLVelocityFilter的INIT、RECOVERING、VALID、INVALID状态承担连续性判断：项目笔记描述恢复至少三帧且跨越100 ms，约500 ms缺测进入失效。恢复时重建增量时间基准，不能拿断流前后的两帧乘整个断流间隔。否则接口会虚构一段没有观测覆盖的位移。

一条时间线解释延迟缓存与输出预测器

假设观测平均速度代表9.95 s，10.03 s收齐，10.05 s处理。若直接和10.05 s状态比较，在1 m/s²加速度下会出现约0.10 m/s的假创新；再调 $R$ 只能掩盖时间错误。正确对象是9.95 s的状态、协方差和观测三者。可手写的精确做法是回到保存的历史快照，更新后重放IMU及之后的观测；工程上还可维持统一延迟融合时域，并用当前输出预测器传播和整合修正，避免每次完整重放。

项目中 AP_NavEKF3_Measurements.cpp 形成观测时间，EKF_obs_buffer::recall召回适合融合时域的样本，UpdateFilter在该时域计算，calcOutputStates把修正反馈到当前输出。官方EKF架构说明采用延迟时域与输出预测器分工。TB02-S2 缓存长度要覆盖允许延迟；超出历史范围的样本应丢弃或进入专门迟到处理，而不是夹到最早时刻假装准时。输出平滑解决的是控制连续性，不代表历史状态没有修正。

用完整时间线走一遍：更新过去，为什么能改善现在？

设当前控制时刻为 $t_c=10.05\ \mathrm s$ ，DVL观测代表 $t_m=9.95\ \mathrm s$ 。在 $t_m$ ，保存的预测速度为 $0.30\ \mathrm{m/s}$ ，方差为 $0.04\ \mathrm{m^2/s^2}$ ；观测为 $0.22\ \mathrm{m/s}$ ，方差为0.25。先在这个历史时刻用标量KF得到速度 $0.2889655\ \mathrm{m/s}$ ，不是拿它直接替换当前速度。

从 $t_m$ 到 $t_c$ 之间如果仍有已记录的 $1\ \mathrm{m/s^2}$ 加速度，那么重新推进0.10 s后，当前速度应为 $0.3889655\ \mathrm{m/s}$ 。旧的当前预测则是 $0.30+0.10=0.40\ \mathrm{m/s}$ 。这次观测对当前速度的修正仍为 $-0.0110345\ \mathrm{m/s}$ ，但绝对速度没有退回历史值。若错误地把历史测量0.22直接与当前预测0.40比较，便得到−0.18的创新，把真实的后续加速也误当作估计误差。

对于最简单的匀速误差传播，历史修正如何传到现在可以写得很清楚。令 $\Delta p_m,\Delta v_m$ 为历史位置、速度修正， $T=t_c-t_m$ 为传播时长，则

\begin{bmatrix}\Delta p_c\\\Delta v_c\end{bmatrix} =\begin{bmatrix}1&T\\0&1\end{bmatrix} \begin{bmatrix}\Delta p_m\\\Delta v_m\end{bmatrix}.

第一行说当前位置修正包含历史位置修正，以及速度修正在T秒内积出的额外位移；第二行说匀速误差模型中速度修正保持不变。若 $\Delta p_m=0$ 、 $\Delta v_m=-0.0110345$ 、 $T=0.10$ ，当前位置也应修正约 $-0.00110345\ \mathrm m$ 。真实惯导还包含姿态、偏置与加速度的耦合，不能始终沿用这张二维矩阵；它只是把“历史修正传播到当前”变成能手算的过程。

两种可实现架构，解决的是同一个时间一致性要求

历史回滚与重放适合第一份离线程序。环形缓存保存样本时刻的名义状态、 $P$ 、IMU区间以及融合记录。收到迟到观测时，找到相应快照，在正确时刻预测或插值输入、做一次更新，然后按时间顺序重放后续IMU和后续观测。快照必须明确是在某个观测更新之前还是之后，避免同一观测应用两次。姿态插值不能逐元素线性插值旋转矩阵； $P$ 也不能仅为了“对上时间”随意做元素插值，应从已有时刻按误差模型传播。

固定延迟融合时域加当前输出预测器把主滤波器维持在稍旧的统一时域，使多个传感器在该处对齐。另一路用新IMU推进当前输出；延迟滤波器得到修正后，将差异反馈到当前预测器。这样控制器可以拿到当前估计，而不是直接把一条旧状态用于闭环。输出误差反馈的平滑能避免突跳，但平滑本身不能取代历史协方差更新。

上游采用延迟时域与输出预测的分工；教学C++时间缓存题仅练习找到目标时刻及插值样本，不把它包装成完整的重放滤波器。无论采用哪种架构，都需要给缓存制定边界：历史覆盖不足时不能把超老观测硬塞到最早快照；设备时钟重启时不能继续使用旧的时钟偏移；重复序号不能反复消费；断流恢复不能把整个空白区间当作连续积分。

将整条链写成你自己的处理顺序

收帧、校验、解析，记录采样与接收时刻；非法值到此终止。
统一单位与安装方向，计算质量，更新连续有效性状态机。
仅用连续有效的区间构造增量与代表时刻；恢复首帧只重建时间基准。
将有时间、有杆臂、有误差参数的观测入队。
在历史融合时域召回，构造同一时刻的 $h,H,R_{\mathrm{obs}}$ 。
检查创新门限，接纳后更新均值与 $P$ ；拒绝则不缩小 $P$ 。
将有效修正传播到当前输出，连同有效性与观测年龄交给控制层。

离线有限差分：用可运行程序抓住两个正号

先独立手写：用扰动实验检查H，而不是相信抄下来的正负号

实现 $h_b(R,v,b_g)$ ，分别扰动15个误差分量，用中心差分检查每一列 $H$ 。
刻意把右乘小旋转换成左乘，观察哪一块开始不一致，再解释差异所属坐标系。
对误差重置映射做中心差分，和 $J_r(a)$ 比较；将a缩小一半，检查一阶近似误差约缩小四倍。
在时间缓存练习中先完成样本时间插值。它练的是找到同一时刻，不是完整KF回滚；真正重放还必须携带历史状态、 $P$ 以及之后的输入和观测。

参考实现与运行检查（C++17，先完成练习再展开）

用 g++ -std=c++17 -O2 main.cpp -o main && ./main 编译运行。以下是离线教学实现，不连接传感器或执行器。

#include <vector>
#include <cmath>
#include <cassert>
#include <algorithm>
#include <iostream>
using V=std::vector<double>;
using M=std::vector<V>;
M zero(int r,int c){return M(r,V(c,0.));}
M eye(int n){M a=zero(n,n);for(int i=0;i<n;++i)a[i][i]=1;return a;}
M tr(const M&a){M b=zero(a[0].size(),a.size());for(size_t i=0;i<a.size();++i)for(size_t j=0;j<a[0].size();++j)b[j][i]=a[i][j];return b;}
M mul(const M&a,const M&b){M c=zero(a.size(),b[0].size());for(size_t i=0;i<a.size();++i)for(size_t k=0;k<b.size();++k)for(size_t j=0;j<b[0].size();++j)c[i][j]+=a[i][k]*b[k][j];return c;}
V mv(const M&a,const V&b){V c(a.size());for(size_t i=0;i<a.size();++i)for(size_t j=0;j<b.size();++j)c[i]+=a[i][j]*b[j];return c;}
M skew(const V&a){return {{0,-a[2],a[1]},{a[2],0,-a[0]},{-a[1],a[0],0}};}
V cross(const V&a,const V&b){return mv(skew(a),b);}
double norm(const V&a){double s=0;for(double v:a)s+=v*v;return std::sqrt(s);}
M Exp(const V&a){
    const double t=norm(a);M A=skew(a),A2=mul(A,A),R=eye(3);
    const double c1=t<1e-6?1-t*t/6:std::sin(t)/t;
    const double c2=t<1e-6?.5-t*t/24:(1-std::cos(t))/(t*t);
    for(int i=0;i<3;++i)for(int j=0;j<3;++j)R[i][j]+=c1*A[i][j]+c2*A2[i][j];
    return R;
}
M Jr(const V&a){
    const double t=norm(a);M A=skew(a),A2=mul(A,A),J=eye(3);
    const double c1=t<1e-6?.5-t*t/24:(1-std::cos(t))/(t*t);
    const double c2=t<1e-6?1./6-t*t/120:(t-std::sin(t))/(t*t*t);
    for(int i=0;i<3;++i)for(int j=0;j<3;++j)J[i][j]+=-c1*A[i][j]+c2*A2[i][j];
    return J;
}
void block(M&A,int row,int col,const M&B,double scale=1){
    for(size_t i=0;i<B.size();++i)for(size_t j=0;j<B[0].size();++j)A[row+i][col+j]=scale*B[i][j];
}

V Log(const M&R){
    V a{(R[2][1]-R[1][2])/2,(R[0][2]-R[2][0])/2,(R[1][0]-R[0][1])/2};
    double t=std::acos(std::clamp((R[0][0]+R[1][1]+R[2][2]-1)/2,-1.,1.));
    if(t>1e-7)for(double&v:a)v*=t/std::sin(t);return a;
}
int main(){
    M R=Exp(V{.2,-.1,.3});V v{.4,-.2,.1},bg{.01,-.02,.03},wm{.1,.2,.3},ell{-.36,0,.125};
    auto observation=[&](const V&d){
        M Rt=mul(R,Exp(V{d[6],d[7],d[8]}));V vt=v,omega(3);
        for(int i=0;i<3;++i){vt[i]+=d[3+i];omega[i]=wm[i]-bg[i]-d[12+i];}
        V h=mv(tr(Rt),vt),lever=cross(omega,ell);
        for(int i=0;i<3;++i)h[i]+=lever[i];return h;
    };
    M H=zero(3,15);block(H,0,3,tr(R));block(H,0,6,skew(mv(tr(R),v)));block(H,0,12,skew(ell));
    const double eps=1e-6;double maxerr=0;
    for(int j=0;j<15;++j){
        V plus(15),minus(15);plus[j]=eps;minus[j]=-eps;
        V hp=observation(plus),hm=observation(minus);
        for(int i=0;i<3;++i)maxerr=std::max(maxerr,std::abs((hp[i]-hm[i])/(2*eps)-H[i][j]));
    }
    assert(maxerr<1e-8);
    V a{.02,-.03,.01},negative=a;for(double&v:negative)v=-v;
    M J=Jr(a);double reseterr=0;
    for(int j=0;j<3;++j){
        V ap=a,am=a;ap[j]+=eps;am[j]-=eps;
        V rp=Log(mul(Exp(negative),Exp(ap))),rm=Log(mul(Exp(negative),Exp(am)));
        for(int i=0;i<3;++i)reseterr=std::max(reseterr,std::abs((rp[i]-rm[i])/(2*eps)-J[i][j]));
    }
    assert(reseterr<1e-9);
    std::cout<<"H error="<<maxerr<<" reset error="<<reseterr<<'\n';
}

补充练习答案与阅读提示

练习：将代码右乘改成左乘，保留旧 $H$ ，观察测试失败；再设v=0，解释姿态块变零。答案：左扰动导数属于另一切空间；速度为零时，单次DVL平移速度对姿态没有直接一阶敏感度，但历史运动耦合、杆臂与其他传感器仍可能提供信息。第二题：给同一观测人为增加100 ms到达延迟，历史融合结果应只改变可用时刻，不应改变它代表的物理时刻。

源码精读：上游接口的每一步都对应一个物理选择

本节把两条线放在一起读：项目报告描述自定义DVL解析、质量与恢复状态机；下面的短代码摘录来自官方 ArduSub-4.1.1 上游，标签提交 66c674c1927d5d31401e6b4bc48b8eb3ee7e066e。前者告诉我们项目怎样组织输入，后者让我们看到真实C++接口怎样把输入送入估计器。源码归ArduPilot贡献者所有，按仓库 GNU GPL v3许可引用；下列保持原代码，不是教学伪代码。

A．`writeBodyFrameOdom`：把增量变成区间平均速度

出处：AP_NavEKF3_Measurements.cpp，NavEKF3_core::writeBodyFrameOdom，约第129—143行。

    // subtract delay from timestamp
    timeStamp_ms -= delay_ms;

    bodyOdmDataNew.body_offset = posOffset.toftype();
    bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
    bodyOdmDataNew.time_ms = timeStamp_ms;
    bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
    bodyOdmMeasTime_ms = timeStamp_ms;

    // simple model of accuracy
    // TODO move this calculation outside of EKF into the sensor driver
    bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);

    storedBodyOdm.push(bodyOdmDataNew);

第一步从传入时间减去 delay_ms，是毫秒时间轴上的修正；区间代表时刻如何定义应由调用链统一负责，不能看到这里减延迟，就再无条件补一次相同延迟。body_offset 保存传感器安装位置，稍后与IMU偏移相减形成真正杆臂。

delPos 的单位为米、delTime 为秒，除法结果自然是米每秒。该行是“这一路融合速度而非绝对位置”的直接代码依据。delAng/delTime 类似地形成角速度字段。time_ms 与 vel 一起写入对象，意味着队列里的不是一个无时间的数字，而是某个过去区间的观测。

velErr 的式子在质量100时取最小标准差，质量0时取最大标准差。项目算例使用最小0.1、最大0.9，于是得到 $0.9-0.8q/100$ ；这不是把质量本身当成方差。最后 push 只表示保存待融合观测，尚未执行状态修正。前面的函数入口还会拒绝NaN、过小积分间隔以及尚未初始化的状态，完整阅读时应先理解这些前置条件。

B．`SelectBodyOdomFusion`：把观测召回到延迟IMU时域

出处：AP_NavEKF3_PosVelFusion.cpp，第1901—1906行。

    // Check for body odometry data (aka visual position delta) at the fusion time horizon
    const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms);
    if (bodyOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) {

        // Fuse data into the main filter
        FuseBodyVel();
    }

关键自变量是 imuDataDelayed.time_ms，不是“函数现在执行的墙钟时间”。这让观测、延迟状态与延迟协方差围绕同一融合时域工作。recall 返回可融合数据还不够，源配置也必须允许这一观测。只有两项条件均满足，才进入 FuseBodyVel。因此调度器调用过函数不等于发生了一次有效观测更新。

C．`FuseBodyVel`：先解释转动速度，再计算创新

出处：同文件，第1257—1267行。

        bodyVelPred = (prevTnb * stateStruct.velocity);

        // correct sensor offset body frame position offset relative to IMU
        Vector3F posOffsetBody = bodyOdmDataDelayed.body_offset - accelPosOffset;

        // correct prediction for relative motion due to rotation
        // note - % operator overloaded for cross product
        if (imuDataDelayed.delAngDT > 0.001f) {
            bodyVelPred += (imuDataDelayed.delAng * (1.0f / imuDataDelayed.delAngDT)) % posOffsetBody;
        }

prevTnb 把导航系速度变成机体系速度，对应我们的 $\hat R^T\hat v$ 。body_offset-accelPosOffset 明确以IMU位置而不是任意机体原点为杆臂起点。delAng/delAngDT 是该延迟IMU区间的角速度，% 在这里重载为叉乘，最后一行就是 $\omega\times\ell$ 。条件保护避免极小时间间隔的除法；它也提醒读者，正确的旋转补偿要求角增量和角增量时间来自同一个区间。

上游用其完整状态向量生成观测导数，并逐轴处理。本章15维右扰动推导用来学习局部误差几何；阅读上游时应按它自己的四元数状态索引理解长导数，不把本章 $H$ 的列号直接套到源码数组。

D．创新符号不是争论方向，而是整条更新必须一致

出处：同文件第1391行与第1867—1870行（两处独立短摘录）。

            innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x;

                    statesArray[j] = statesArray[j] - Kfusion[j] * innovBodyVel[obsIndex];

源码定义 $\nu=h-z$ ，所以使用减去 $K\nu$ ；教材定义 $r=z-h$ ，所以加上 $Kr$ 。因为 $\nu=-r$ ，二者修正相同。将预测0.30、观测0.22代入：源码创新是+0.08，减去正修正；教材创新是−0.08，加上负修正。两者都让速度下降。不要只改其中一个符号。