IMU、DVL与位置观测如何协同：从卡尔曼原理到异步融合

先回答最容易混淆的三个问题

卡尔曼滤波是在把传感器的协方差变小吗？ 通常不是。传感器本身仍然有原来的测量噪声；滤波器结合已有信息与新观测，让“我们对机器人状态的估计”更可靠。传感器噪声协方差与状态估计误差协方差是两件事。

必须有两个以上传感器才需要卡尔曼滤波吗？ 不需要。只有一个摄像头连续给出目标位置，也能结合运动模型估计速度、预测下一帧位置。多个传感器只是“增加观测信息”的一种途径。

IMU、DVL、USBL/GPS到底谁预测、谁更新？ 在常见的惯导辅助架构中，IMU 的角速度与比力驱动预测；DVL 提供速度观测；USBL 或 GPS 提供位置观测。它们不是轮流担任“主传感器”，而是通过不同的观测方程约束同一份状态和协方差。这个角色由建模方式决定，不是传感器名字的永久属性。

建议先读 卡尔曼基础、误差状态滤波，再把本页作为完整融合链路来走一遍。

1. 为什么要“估计状态”，而不仅仅滤波一个数

1.1 传感器看到的，往往不是控制器真正需要的

IMU 给角速度与比力，控制器需要姿态、地速、位置。DVL 给自身安装点在自身坐标系下的速度，位置控制器需要导航系下机器人参考点的位置。USBL 更新较慢且可能延迟，控制器却每几个毫秒就要一次状态。

我们用状态向量把需要长期记住的物理量组织起来。例如一维运动可以定义：

这里 $p$ 是位置，单位 $\mathrm{m}$；$v$ 是速度，单位 $\mathrm{m/s}$；$b_a$ 是加速度计偏置，单位 $\mathrm{m/s^2}$。状态不是“传感器读数清单”，而是描述系统演化所需的内部变量。

假设加速度读数为 $a_m$，去除偏置后才是实际加速度，则：

第一条是速度的定义；第二条表达“测量值减偏置、减测量噪声才是真加速度”；第三条用小随机变化描述偏置缓慢游走。$n_a$ 与 $n_{ba}$ 分别是测量噪声和偏置驱动噪声，它们不是同一个噪声。

这三条式子创造了信息通路：偏置错了，会导致速度持续积累误差；速度错了，又会导致位置误差逐步扩大。 因而一个位置观测不只能够改位置，也可能反过来帮助估计速度和偏置。

1.2 除多传感器融合之外，卡尔曼滤波还有什么作用

• 估计未直接测量的状态。 从连续位置观测估计速度，从运动误差中识别 IMU 偏置。是否能识别取决于可观测性，不能任意加入状态就期待自动收敛。
• 在观测之间连续输出。 USBL 可能每秒一帧，IMU 每秒数百帧，预测步骤填补中间时刻。填补的是带不确定性的预测，不是凭空增加测量信息。
• 短时预测。 给控制器、目标跟踪器预测下一个时刻的位置，补偿计算或执行延迟。
• 量化不确定性。 用协方差告知控制器“当前横向位置可靠到什么程度”，从而限速或退出精确悬停。
• 估计慢变参数或扰动。 把偏置、海流或外力加入合适的模型后，可以构造扩展状态观测器；这里需要可辨识的模型和足够观测，KF 本身不会自动识别所有扰动。
• 检测不一致观测。 比较观测和预测差异是否与当前不确定性相称，辅助拒绝异常。标准 KF 并不天然抗所有离群值，鲁棒门限是额外机制。

“滤波”使用当前和过去数据；“平滑”还用未来数据回头修正过去轨迹。普通在线 KF 不是离线平滑器，不能把两者的结果混为一谈。

2. 三种协方差：先把各自管什么分清楚

严格地说，$P=\operatorname{Cov}(x-\hat x)$，其中 $\hat x$ 是估计，$x$ 是真值。对一维状态，若位置标准差为 $0.2\,\mathrm{m}$，对应位置方差是 $0.04\,\mathrm{m^2}$，不是 $0.2$。

矩阵中的非对角元素同样重要。位置与速度交叉协方差 $P_{pv}$ 的单位为 $\mathrm{m^2/s}$；它不是“相关系数”，既可以为正，也可以为负。

2.1 用一个完全能手算的例子推导权重

假设预测位置为 $\hat p^-=10\,\mathrm{m}$，预测方差为 $P^-=4\,\mathrm{m^2}$。USBL 报告 $z=12\,\mathrm{m}$，观测方差为 $R=1\,\mathrm{m^2}$。上标 $-$ 表示尚未融合本帧，上标 $+$ 表示融合之后。

在两种误差独立、零均值、高斯的假设下，希望选择的位置既不要离预测太远，也不要离观测太远：

分母不是装饰。某项方差越大，同样大小的偏差越容易被允许；它对最终结果的拉力越小。对 $p$ 求导并令其等于零：

把含 $p$ 的项收集起来：

解出 $p$ 并整理成“预测加修正”：

这里 $K$ 是卡尔曼增益，控制本次采用观测残差的比例。代入数值，$K=4/5=0.8$，所以 $\hat p^+=10+0.8(12-10)=11.6\,\mathrm{m}$。

融合后的方差为：

变小的是估计方差，从 $4$ 变为 $0.8$；USBL 的观测方差仍然是 $1$。 下一帧 USBL 不会因为上一次融合而突然变成更好的传感器。

在线性、正确噪声模型下，同一时刻增加独立有效观测不会增大后验协方差；但预测会注入过程噪声，误差可能继续增长。EKF 的线性化、错误外参或过小噪声设置，也可能造成“报告方差很小，实际误差很大”的不一致。不能以 $P$ 越小作为唯一调参目标。

3. 为什么 GPS 的位置观测也能修正速度

假设先只估计位置和速度：

矩阵各项的单位不同：左上是 $\mathrm{m^2}$，右下是 $\mathrm{m^2/s^2}$，交叉项是 $\mathrm{m^2/s}$。位置观测为 $z=p+n_p$，因此：

$H_p$ 的意思很直接：传感器直接看到第一维位置，没有直接看到第二维速度。假设 $R_p=1$，创新协方差和增益分别为：

若预测为 $\hat x^-=[10,1]^T$，位置观测仍是 $12\,\mathrm{m}$，创新为 $r_p=2\,\mathrm{m}$，则：

速度为什么也从 $1$ 变成 $1.4\,\mathrm{m/s}$？因为 $P_{vp}=1$ 告诉滤波器：在先前传播中，位置误差和速度误差已经有统计关联。机器人比预测走得更远，可能不只是位置估计错，也可能是速度一直偏低。滤波器按已积累的相关性分配修正，而不是硬把位置观测值复制给位置变量。

融合后：

3.1 交叉协方差从哪里来

假设初始位置和速度互不相关，$P_0=\operatorname{diag}(\sigma_p^2,\sigma_v^2)$。经过 $\Delta t$ 的匀速传播：

初始为零的交叉项变成 $\Delta t\sigma_v^2$，原因是位置预测用了速度。因此“位置更新影响速度”来自运动方程，不是软件工程师额外加了一条经验补偿。

对上一节的位置、速度、偏置状态，忽略一个间隔内偏置的随机变化，有：

最后一列解释了偏置如何影响位置、速度。观测积累后会产生相应的交叉协方差，于是 DVL 或位置更新能够修正偏置。若运动与观测不能区分某些误差，这条通路仍可能很弱，甚至不可观测。

4. 把机器人状态统一起来，而不是把传感器两两平均

4.1 统一坐标、参考点和单位

本页采用局部导航系 $n$ 为 NED（北、东、下），机体系 $b$ 为 FRD（前、右、下），状态位置对应 IMU 参考点。$C_b^n$ 把机体系分量变成导航系分量，$C_n^b=(C_b^n)^T$。用 $C$ 表示旋转矩阵，避免与观测协方差 $R$ 混淆。

局部小范围、较短时间的教学模型忽略地球自转、运输角速度和重力变化；远航或高精度惯导需要加入这些项。GPS 经纬度必须先转换到统一的局部坐标，不能把“度”直接与 USBL 的“米”相减。

名义状态为：

位置、速度、加速度偏置、陀螺偏置各占三维；姿态四元数存四个数，但只有三个独立自由度。因此名义状态存 $16$ 个数，误差状态为 $15$ 维：

姿态误差使用右乘、小角度定义：$C_{b,\mathrm{true}}^n=\hat C_b^n\operatorname{Exp}(\delta\theta^b)$。矩阵 $[a]_\times$ 满足 $[a]_\times b=a\times b$。这一约定决定后面雅可比的正负号，不能与左乘误差版本混用。

4.2 预测：IMU告诉我们“这段时间发生了怎样的运动”

加速度计测量的是比力，不是直接的导航系加速度。定义去偏后的输入：

角速度单位为 $\mathrm{rad/s}$，比力单位为 $\mathrm{m/s^2}$。NED 重力为 $g^n=[0,0,g]^T$。静止且机体与 NED 对齐时，加速度计理想比力约为 $[0,0,-g]^T$；加上重力后才得到零运动加速度。

在一小段 $\Delta t$ 内，先用姿态把比力旋转到导航系：

然后积分：

姿态用旋转增量传播：

$\otimes$ 是四元数乘法。若旋转向量为 $\phi$，则 $\operatorname{Exp}_q(\phi)=[\cos(\|\phi\|/2),\;\sin(\|\phi\|/2)\phi/\|\phi\|]$，零角度用极限处理。实际高精度积分会使用中点、圆锥与划桨补偿，本式用于看清信息方向。

预测不只更新均值，还必须同步更新误差协方差：

第一项传播已有误差，第二项加入本次 IMU 噪声和偏置变化带来的新误差。只积分状态而不传播 $P$，后面的融合权重就没有正确依据。

4.3 误差方程为什么有姿态、偏置耦合项

在上述右乘误差约定下，连续误差传播的一阶矩阵为：

每个块都是 $3\times3$。第二行说：姿态错了，比力投影方向就错；加速度偏置错了，加速度也错。第三行说：陀螺偏置错了，姿态误差会积累。$w$ 按加速度白噪声、陀螺白噪声、加速度偏置游走、陀螺偏置游走排列时：

若连续噪声谱密度为 $Q_c$，时间内近似 $A,L$ 不变，则：

小步长一阶近似为 $\Phi\approx I+A\Delta t$、$Q_d\approx LQ_cL^T\Delta t$。连续噪声谱密度和离散样本方差不是同一参数，不能把它们都叫“IMU方差”后随意代入。详细误差推导见 ESKF。

4.4 “IMU十五维”不是IMU输出十五路数据

物理 IMU 通常提供三轴角速度和三轴比力，共六个运动测量分量，外加时间戳、温度、状态等信息。十五维指的是整个惯性导航估计器的误差状态，不是 IMU 硬件输出维数。这里统一按位置、速度、姿态误差、加速度偏置、陀螺偏置排列，与前面的 ESKF 章节一致。

为什么不同传感器数据维数不一样，仍能进入同一个滤波器？因为观测函数把状态映射到传感器空间，而增益把该空间的残差映射回状态误差空间：

此处 $m_i$ 是当前观测包含的有效标量数量，不是固定等于状态维数。深度取 $m_i=1$，三轴 DVL 取 $m_i=3$，仅水平位置取 $m_i=2$，三维位置加三维速度可取 $m_i=6$。没有测量的轴不必补零。

例如三维 DVL 更新时，$P H_D^T$ 是 $15\times3$，$S_D^{-1}$ 是 $3\times3$，所以 $K_D$ 是 $15\times3$；乘三维速度残差就得到十五维误差修正。输入不需要“凑成十五维”。

融合之后，控制器得到的主要输出仍是位置、速度和姿态；偏置估计供下一次 IMU 去偏，协方差及状态标志用于健康检查。它不会输出一个混杂单位的“融合后的传感器数值”。具体的 DVL 多普勒、GPS 位置与伪距、USBL 声学时间模型，继续读 传感器输出与观测建模。

5. 更新：每一种传感器都要写自己的“预测读数”

统一形式是：

$z_i$ 是实际读数；$h_i(x)$ 回答“如果机器人状态真是当前估计，这个传感器应该读到什么”；$R_i$ 表示该读数有多不可靠。融合的核心就是比较实际读数与预测读数，而不是比较两个不同物理量。

5.1 DVL：速度更新，不是位置更新

令 $d$ 是 DVL 坐标系，$C_b^d$ 是已标定的安装旋转，$\ell_D^b$ 是从 IMU 指向 DVL 测量点的杆臂。底跟踪时，DVL 观测函数是：

括号里第一项是 IMU 原点的机体系地速；第二项是旋转导致 DVL 安装点多出来的速度。例如角速度 $0.5\,\mathrm{rad/s}$、垂直于转轴的杆臂 $0.2\,\mathrm{m}$，就会产生 $0.1\,\mathrm{m/s}$ 速度；不能误判成机器人平移。

若已经把 DVL 数据转到机体系，并且确实可忽略杆臂，观测函数简化为 $h_D=C_n^bv^n$。右乘误差下，雅可比是：

姿态块为什么是正号？因为 $(\hat C_b^n\operatorname{Exp}(\delta\theta))^T\approx(I-[\delta\theta]_\times)\hat C_n^b$，且 $-[\delta\theta]_\times u=[u]_\times\delta\theta$。

保留杆臂和 DVL 原坐标时，陀螺偏置块还会出现 $C_b^d[\ell_D^b]_\times$，因为 $(-\delta b_g)\times\ell_D^b=[\ell_D^b]_\times\delta b_g$。完整观测模型还需把角速度噪声带入等效观测噪声，并考虑它与传播噪声可能相关。

水跟踪不能直接套底跟踪。 若水流速度为 $v_c^n$，速度项变为 $C_n^b(v^n-v_c^n)$。它约束的是相对水体速度；若不估计或已知海流，不能冒充地速，否则海流会被错误解释为机器人漂移。

5.2 USBL/GPS：位置更新，但先问“谁的位置”

若数据已转换为导航系中接收器的位置，杆臂为 $\ell_P^b$，则：

机器人转动时，天线或声学应答器会绕 IMU 移动；这个运动不是 IMU 原点的位置变化。对应雅可比：

如果杆臂为零，便是 $[I,0,0,0,0]$。虽然它没有直接测速度和偏置，增益中仍可通过交叉协方差修正这些状态，正如第三节的数字例子。

USBL 经常给母船/阵列坐标系中的相对位置。先要用母船位置、阵列姿态与安装外参转换到统一导航系；母船的 GPS 或姿态误差也属于这个位置观测的不确定性，不能仅填写 USBL 厂商给出的单独精度。

GPS 通常用于水面、天线可接收卫星信号的阶段。水下 ROV 的声学定位不是“水下GPS收星”，只是能提供类似的绝对或参考坐标位置约束。若 GPS 还给多普勒速度，可以另建速度观测；并非所有 GPS 输出都只能用于位置。

5.3 深度：只约束相应方向，不必伪造完整三维位置

令 $e_3=[0,0,1]^T$，深度传感器杆臂为 $\ell_Z^b$，参考水面深度为零：

这是一维观测，更新使用一行雅可比。不能为了凑三维向量，把没有测量的水平位置填写成零，再赋予很小方差。缺少某个维度的直接观测，就不伪造该维度的观测；但经由交叉协方差，其他相关状态的修正仍可能非零。

6. 一帧观测怎样进入同一个滤波器

对已经预测到测量时刻的状态，先算创新：

创新不是传感器值本身，而是“实际读数减预测读数”。然后线性化：

由两项独立噪声的协方差相加，得到：

$S_i$ 描述“在当前状态误差和传感器误差共同作用下，创新通常有多大”。随后求增益和误差修正：

工程代码通常用 Cholesky/LDLT 解线性方程，避免显式求逆。$K_i$ 的每一行对应一个状态，所以只有三维位置观测，也能产生十五维误差修正。

误差注入名义状态：位置、速度、偏置直接相加，姿态按同一右乘约定相乘：

协方差可采用数值更稳的 Joseph 形式：

ESKF 注入后还要把误差重新定义为相对新名义状态的小误差，即“重置”。一阶姿态重置雅可比约为 $G_\theta=I-\tfrac12[\delta\hat\theta]_\times$，其余对角块为单位阵；执行 $P^+=GP_{\rm upd}G^T$，误差均值归零。归零的是已注入的误差均值，不是把协方差清零。

7. 哪个先融合：按测量时间，而不是等所有传感器到齐

假设以下频率仅用于说明：IMU 为 $200\,\mathrm{Hz}$，DVL 为 $10\,\mathrm{Hz}$，USBL 为 $1\,\mathrm{Hz}$。

1. 每个 IMU 间隔把状态和协方差向前传播。
2. DVL 在 $t=1.000\,\mathrm{s}$ 测得速度，则用同一时刻的状态预测 DVL 读数，做速度更新。
3. USBL 在 $t=1.000\,\mathrm{s}$ 测得位置，也用该时刻状态做位置更新。
4. 两次更新之后继续处理后续 IMU，不需要等下一帧 USBL 才输出状态。

7.1 同一时刻可以顺序更新，也可以堆叠更新

独立的 DVL 与位置观测可堆叠为：

也可以先更新 DVL，再用更新后的状态、协方差更新位置。在线性、同一时间、噪声独立且处理一致时，两者等价。 非线性 EKF 若每次重新线性化，顺序可带来小差异；不同时间的观测本来就应在各自时刻处理，不能简单一次堆叠。

若两个来源共享同一份数据，例如 USBL 输出已用同一个 GPS 修正，或者“DVL里程计位置”只是当前已融合 DVL 速度的积分，则二者可能相关。此时 $R$ 的非对角交叉块未必为零。把同一条信息当独立观测重复使用，会让协方差虚假缩小。尤其是已经融合过的 DVL 再积分成位置回灌时，新增观测还与当前先验误差相关；仅补上堆叠观测矩阵的噪声交叉块未必足够，还须处理先验误差与观测噪声的相关性。工程上优先避免这种重复使用。

7.2 预测与更新不是传感器的固有身份

也可以用推进器动力学预测，再把 IMU 加速度作为观测；也可以估计二维位置，用 DVL 速度作为运动输入。两种设计都可能成立，但状态、过程噪声和观测噪声必须重新推导。

对于已有 IMU 惯导链路，最自然的基线是“IMU传播，DVL/深度/位置修正”。不要在同一个滤波器里既把某份 DVL 当独立输入，又把同一份数据当独立测量更新，却不处理相关性。

8. USBL迟到两秒，不能直接拿来纠正现在的位置

必须区分：采样代表时刻 $t_m$、到达软件时刻 $t_a$ 和当前滤波时刻 $t_f$。声学传播、设备解算、通信和排队会造成 $t_a>t_m$。

例如机器人以 $0.5\,\mathrm{m/s}$ 运动，迟到 $2\,\mathrm{s}$ 的 USBL 位置与现在相差约 $1\,\mathrm{m}$。直接拿它更新现在状态，即使 USBL 完全无噪声，也会把机器人往旧位置拉。

8.1 回滚与重放

维护状态、协方差、IMU 以及已经融合观测的历史缓存。迟到观测到达后：

1. 找到 $t_m$ 之前的检查点。
2. 重放输入到 $t_m$，必要时切分跨越该时刻的 IMU 间隔。
3. 在 $t_m$ 融合 USBL。
4. 按时间顺序重新执行后面的 IMU 和观测，恢复到当前时刻。

必须同时回滚状态和协方差。还必须重放之后已经发生的观测，否则会丢失它们；每帧要有唯一标识，防止重复融合。缓存外的旧观测不能随意冒充新观测，可按明确策略丢弃并记录。

8.2 延迟融合时域与当前输出预测器

另一种设计让主滤波器落后当前时间一小段，使不同传感器能在共同历史时刻融合，再用单独的输出预测器推到当前，供控制器使用。ArduPilot 的官方 EKF2 设计说明明确介绍了延迟时域与输出预测器；学习时要把“主滤波状态”和“给控制器的当前输出”分开。官方设计说明

两个方案解决同一个时间一致性问题，计算开销与实现复杂度不同。不能仅把时间戳改成当前时间，称作完成了“延迟补偿”。固定延迟参数也不能替代真正的时钟同步；设备时间与飞控时间要先做时钟偏移、必要时漂移映射。

9. 失效与异常：滤波器没有制造信息的能力

9.1 创新门限到底检验什么

归一化创新平方为：

其无量纲值衡量本次残差相对预测不确定性有多离谱。在近似高斯、模型一致时，可按观测维数选择卡方阈值。三维观测不能机械照搬一维阈值；把阈值加大不是修复时间戳和坐标错误。

异常检测前先做格式、有限数、时间戳、坐标系、DVL底锁/有效波束、USBL质量等检查。连续大创新可能是坏观测，也可能是滤波器已经漂了或外参错了；不能永远拒绝重新恢复的真实位置。

9.2 DVL失效时

停止 DVL 更新，继续 IMU 传播，并融合仍可用的深度和 USBL。速度与位置不确定性通常增长，控制策略应依据误差预算减速、限制悬停控制强度或进入降级模式。

不要把最近一次速度反复发送并当成“新独立观测”。它会让滤波器以为收到了许多一致证据，造成过度自信。滑窗外推可以用作降级控制参考，但若作为滤波观测，必须建模其时间相关性、与 IMU 的相关性及误差增长，不能沿用正常 DVL 的小 $R$。

9.3 USBL失效但DVL还好时

速度仍然受到约束，位置漂移可能显著慢于纯惯导，但没有凭空出现绝对水平位置锚点。若把整条轨迹在水平面整体平移，IMU、DVL、深度读数仍可保持不变，这说明绝对水平平移不可观测。

同理，没有外部航向基准或足够几何信息时，全局航向也可能不可观测；“DVL能测速度”不等于“始终能估出所有姿态和所有偏置”。进一步阅读 可观测性。

9.4 恢复时

先确认测量时刻、参考坐标与外参没有变化，再检查创新。对连续有效数据做恢复确认，按业务设计采用正常融合、增大不确定性后重收敛，或明确的位置重置。重置后的控制目标也要保持物理意义一致，防止位置坐标跳变导致控制器突然发大推力。

10. 位置跳变：速度积分与绝对位置差很多，到底相信谁

10.1 先区分三个对象，避免一开始就问错问题

• DVL 原始观测是速度，不是一个与 USBL 平行的绝对位置。
• 由惯导传播并经过 DVL 更新维护的位置，是同一估计器的历史结果，已包含以前的观测信息。
• 新来的 USBL/GPS 位置，是在特定测量时刻对这个估计器的新约束。

所以不能不加说明地把“DVL积分位置”和“USBL位置”当成两个独立传感器的等权位置。应该比较同一参考点、同一坐标系、同一测量时刻的预测位置与绝对观测，并使用正确的先验协方差和观测协方差。

10.2 同样相差两米，可能应该采取完全不同的动作

考虑一维位置，滤波器预测为 $100\,\mathrm{m}$，新位置观测为 $102\,\mathrm{m}$，因此创新都是 $r=2\,\mathrm{m}$。下表只演示位置分量，真实滤波器还会通过交叉协方差修正速度、姿态和偏置。

前两种情况下，$S=4.04$，归一化创新平方为 $\gamma=4/4.04\approx0.99$，在各自的误差预算下并不离谱。第三种情况却得到 $S=0.08$、$\gamma=50$，说明两份“高置信”信息强烈冲突。

卡尔曼增益负责合理信息之间怎样折中，创新检验负责判断这帧是否适合进入正常折中。 不能只问“绝对位置是不是永远比积分准”，也不能把 $K=0.5$ 当成自动安全的答案。

如果声学位置存在两米固定偏差，它连续十帧都稳定在错误位置，也不代表它是真值。稳定性检验能排除偶发毛刺，但无法消除持续多径、声速误差、母船姿态误差或基准坐标错误。

10.3 为什么“很大偏差”不等于“绝对位置坏了”

创新检验的前提是 $P^-$ 和 $R$ 可信。假如 DVL 丢失两秒，软件仍重复融合旧速度或把过程噪声设得过小，报告的位置方差可能只有 $0.01\,\mathrm{m^2}$，但真实位置已经漂了一米。

若重新收到准确 USBL，$R=0.01\,\mathrm{m^2}$，一米残差得到 $\gamma=1/0.02=50$，正常门限就可能拒绝它。每次都拒绝，新信息永远进不来，于是形成“我因为过度自信而拒绝纠正”的锁死。

正确处理方向是诊断为什么先验过度自信：漏了哪些过程噪声、是否重复计算信息、时间戳是否错误、偏置或水流有没有建模。恢复阶段可以采用有依据的协方差重初始化、增广不确定性或重新定位，但不应为了通过某一帧而随手改大 $P$ 或门限。

同样，把 $R$ 设得很小只能让滤波器更相信传感器，不能让传感器变准；把 $R$ 无限放大虽然会让创新检验更容易通过，但又几乎不修正状态，也不等于完成了恢复。

10.4 NIS门限如何随维数变化

若创新是正确建模的 $m$ 维高斯变量，则 $\gamma=r^TS^{-1}r$ 近似服从 $m$ 自由度卡方分布。选 $99\%$ 累积概率的上界时，一维约为 $6.635$，二维约为 $9.210$，三维约为 $11.345$。NIST 卡方临界值表

这是名义统计条件，不是“真实场景只会误拒绝百分之一”的无条件保证。线性化误差、非高斯多径、相关重复样本都会破坏假设。

三维位置的 $R$ 也未必各向同性。若 USBL 某方向误差较大，协方差椭球应在那个方向更宽；只用欧氏距离阈值，会把精度不同的方向一视同仁。正确的 $S^{-1}$ 相当于按各方向可信度归一化残差，并考虑方向间相关性。

10.5 大偏差先排查什么：按成本从低到高

1. 时间延迟。 看测量代表时刻，不看串口收到时刻；两秒延迟在运动中足以制造米级偏差。
2. 坐标和基准。 NED/ENU、深度/高度、经纬度/米、本地原点、母船坐标/世界坐标有没有混用。
3. 参考点。 USBL 应答器、GPS 天线与 IMU 的杆臂是否补偿；旋转时大偏差是否随姿态相关。
4. 观测质量。 USBL 解算模式、声速假设、多径、母船定位质量；GPS 固定解/浮点解；DVL 底跟踪/水跟踪、波束数和几何是否改变。
5. 先验健康。 外部观测已经丢失多久，偏置是否漂移，姿态误差是否把重力投到水平加速度，协方差增长是否合理。
6. 物理一致性。 连续独立定位点之间的位移是否和 DVL/IMU 支持的运动一致。这里只能检验相对变化，不能单独证明绝对坐标无偏。

日志应同时保存测量时刻、到达时刻、创新、创新方差、门限结果、来源质量、是否底锁、状态机状态与复位原因。只画融合位置曲线，很难分辨坏观测、延迟错配和先验漂移。

10.6 重新定位：把恢复做成状态机，而不是单条if

下面是一套教学设计，不是声称所有飞控使用相同门限。各持续时间、允许漂移和运动限值应由测试与任务风险确定。

“若干帧”应是新测量，不是同一 USBL 解被重复转发。连续定位也可能有强时间相关，因此有效证据量不等于帧数。进入降级可以快，退出通常需要更充分证据，避免边界反复切换，这就是滞环思想。

重新定位时，可信三维位置可以重置位置；没有可靠速度证据时，不能顺便把速度强设为零。若姿态和偏置仍可信，可以保留其估计；若相关状态也不可信，需一起重新初始化适当不确定性。改变某些状态后还要重新处理相应交叉协方差并保证 $P$ 半正定，不能只改一个对角数留下不可能的相关关系。

ArduPilot 的官方故障保护文档也把创新拒绝、长期不一致后的复位、以及不同配置下的创新裁剪分开讨论，并明确裁剪可能增加对异常 GPS 的敏感性。可借鉴“检测与恢复分层”的设计，但 Copter 文档的具体阈值、时间与模式动作不能直接当作本 ROV 的默认配置。官方 EKF 故障保护说明

10.7 位置纠正之后，为什么控制器可能突然猛推

位置控制器看的是误差 $e_p=p_{\rm target}-\hat p$。若估计位置突然修正 $2\,\mathrm{m}$，而目标数值不变，控制器会看到误差突变。它并不知道这是重新定位，可能立即要求较大速度、加速度甚至推进器饱和。

必须区分两件事：

A. 真实估计纠错。 原来以为在 $100\,\mathrm{m}$，现在确认实际在 $102\,\mathrm{m}$。如果任务绝对目标仍为 $100\,\mathrm{m}$，确实需要安全地返回。不能悄悄把目标也改成 $102\,\mathrm{m}$，那会掩盖实际定位误差，并改变任务目标。

B. 纯坐标重定义。 物理位置没变，只是原点或坐标基准变了。在新坐标中，机器人、目标、路径与地图都必须一致变换。此时同步变换目标是为了保持同一物理目标，而不是让机器人“追着坐标跳变跑”。

一个工程组织方法是分离连续的局部里程计坐标与允许全局校正的地图坐标。ROS REP 105 的 odom 与 map 正是这两种语义：前者连续但可漂移，后者承担长期定位但可能跳变。这里引用的是两种坐标系的职责分工，不是照搬轴向：REP 105 默认使用 ENU、z 向上，而本课是 NED、z 向下，接入时必须显式转换。REP 105 官方源码

只考虑平移时，可写成 $p^{map}=p^{odom}+t^{map}_{odom}$。全局校正先更新坐标变换，局部控制仍使用连续运动估计；绝对目标经同一变换映射到局部系，再经过限速、限加速度的参考生成器执行。这样保留了物理目标，又避免把定位跳变直接变成执行器阶跃。含旋转时使用完整刚体变换，不能只加一个偏移量。

另一条路线是飞控输出预测器/校正反馈，但必须让位置、速度、姿态输出及其延迟保持一致。只对位置显示值做低通、速度仍直接用另一份滤波值，会破坏运动学关系；简单限制每次创新大小也不等于一致的状态估计。

10.8 一次完整实例该怎样做

机器人 DVL 短时丢底，USBL 中断二十秒后恢复，第一帧位置比预测偏三米：

1. 不立刻认为 DVL 或 USBL 谁一定错。检查测量时间，把状态对齐到 USBL 时刻。
2. 核对 USBL 参考系、母船位姿及杆臂，检查这帧质量和噪声模型。
3. 若预测方差已合理增长且残差通过检验，正常更新同一状态；位置可能修正较多，速度和偏置按交叉项修正。
4. 若残差不通过，进入恢复候选，查看后续新的定位是否与运动一致，排除固定系统误差；不要无限期机械拒绝。
5. 若证据支持新定位而先验已失真，执行显式重新定位，重建协方差并告知控制器。保留可信速度/姿态，不把一切都清零。
6. 根据任务是绝对航点还是相对悬停，明确目标语义，经过参考限速/限加速度逐步恢复自主运动。

完整答案不是“谁更准就信谁”，而是：先确保比较同一个物理量，再检验双方可信度与残差一致性；正常时做统计更新，严重失配时执行受控恢复。

11. 成熟论文怎么组织这件事

以下论文用于理解设计选择，不把它们的实验结论当作本机的保证。本页公式采用统一的教学约定独立推导，不逐式搬运不同论文的坐标记号。

11.1 基线：先看IMU传播与DVL/深度修正

Potokar、Norman、Mangelson 的 Invariant Extended Kalman Filtering for Underwater Navigation，IEEE RA-L 2021，DOI 10.1109/LRA.2021.3085167，以 IMU 建过程模型、DVL 和深度做修正，并处理 DVL 到 IMU 的坐标与杆臂关系。它进一步用李群误差结构组织滤波，比较的主要证据是 Monte Carlo 仿真，不能称为本文算法已海试。先读传感器结构，再研究为什么更换误差定义可改善线性化性质。作者托管全文

11.2 工程链路：同一滤波器接DVL、深度、声学距离与水面GPS

Krauss、Stilwell 的 Unscented Kalman Filtering on Manifolds for AUV Navigation—Experimental Results，IEEE OCEANS 2022，介绍 UKF-M 的实际 AUV 导航。其传感器包含 DVL、深度、声学距离及水面 GPS，考虑各传感器杆臂、离群值筛选，并有实时海洋机器人平台的外场验证。它的声学输入是距离，不能直接称作 USBL 三维位置。学习重点是：同一状态对每个传感器分别预测读数，融合机制并不因采用 UKF 而消失。论文全文

11.3 从松耦合走向紧耦合：声学原始量也能直接进滤波

Morgado、Oliveira、Silvestre 的 Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles: An experimental validation，Journal of Field Robotics 30(1)，2013。出版页摘要明确说明，它把声学往返传播时间和到达时间差直接放入 EKF，而不只融合 USBL 已解算的位置或距离角度。这里学习的是“先解算位置再融合”和“直接融合原始量”的架构差别；有仿真与海上实验支持。当前这里据出版摘要介绍，不扩写未读取的全文细节。出版页

所谓松耦合，不等于低级：厂商已输出可靠三维位置时，松耦合更容易落地。紧耦合需要原始声学量、精确阵列与声速模型、同步和噪声模型，工程复杂度明显增加。

11.4 学习路线

先手写线性位置—速度 KF；再写统一 ESKF 的 IMU 传播；接 DVL 与深度；接绝对位置与延迟队列；完成异常、恢复与统计检验。最后比较 UKF-M/InEKF，观察初始化误差、非线性和协方差一致性，而不是只追求换一个更高级名字。

四元数及 ESKF 的推导可继续读 Joan Solà 的技术报告，特别留意左右扰动和注入重置的约定。Quaternion kinematics for the error-state Kalman filter

12. 回到 ArduSub：如何沿源码读通

读取固定版本比凭函数名猜测更可靠。下面两行来自 ArduSub-4.1.1 对应提交 66c674c1927d5d31401e6b4bc48b8eb3ee7e066e 的 NavEKF3_core::writeBodyFrameOdom：

bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
storedBodyOdm.push(bodyOdmDataNew);

第一行说明该接口收到增量位移和时间后，构造的是平均速度观测；第二行说明它先进入观测缓冲，而不是立即覆盖位置。不能看到 delPos 这个名字，就认为它给了 EKF 一个独立绝对位置。代码同一函数还处理时间延迟、杆臂、角速度和质量到速度误差的映射。固定提交源码，GPL-3.0

实际阅读按这条链路做笔记：

1. 驱动层： 原始帧是速度还是增量？单位、符号、安装系、有效标志、测量时刻是什么？
2. 入口层： 转成什么 EKF 观测对象？是否设置杆臂、误差、延迟？
3. 缓存层： 观测何时取出？是否与延迟 IMU 状态对齐？
4. 融合层： 预测读数、创新、观测方差、门限分别在哪？注意源码可能用“预测减观测”，与本页相反，增益应用的符号须一起核对。
5. 反馈层： 修正哪些状态？何时投影到当前输出？最后导航系米如何变成控制器需要的 NEU 厘米？

本页十五维误差状态是教学基线，不应机械宣称 ArduSub 实际状态数量、顺序及所有辅助状态都与它完全相同。你需要掌握的是“哪个物理量经哪个方程修正哪个状态”，再对齐具体实现。

13. 本章手撕：让位置和速度真正相互修正

可以先打开 二维卡尔曼在线练习 与 时间戳缓存在线练习，再做下面的扩展题。

题目A：手算一次位置更新

给定第三节的 $\hat x^-=[10,1]^T$、$P^-=[[4,1],[1,1]]$，位置观测 $z=12$、$R=1$。输出更新后的两个状态和四个协方差元素。目标结果应为 $[11.6,1.4]^T$ 与 $[[0.8,0.2],[0.2,0.8]]$。

如果代码只更新位置、速度保持 $1$，说明你忽略了交叉协方差。

题目B：把DVL和位置观测接到同一个更新函数

用二维状态 $x=[p,v]^T$。位置观测行向量为 $H_p=[1,0]$，DVL速度观测为 $H_v=[0,1]$。实现一个通用标量观测更新函数，输入观测值、方差与这两个系数，而不是写两个互不相关的滤波器。

检查：线性同一时刻、独立噪声时，先位置再速度和先速度再位置的结果应在浮点误差内一致。

题目C：理解异步调度

把输入整理为带测量时间的事件。每次事件到来先传播到该测量时刻，然后只更新该事件对应观测；连续两个同时间的事件之间传播间隔为零。乱序到达不能简单按到达顺序传播，更不能使用负时间步长。扩展时加入历史缓存，而不是在基础函数中悄悄截断负时间。

自测清单

• 创新为零，均值不变但协方差通常仍减小。
• 观测噪声很大，修正很小；噪声很小，不代表可以无视异常值。
• 只有速度观测时，不应给未知全局位置凭空设定高置信绝对原点。
• 同一份观测重复更新会错误增加信息；调度器必须防重。
• $R\le0$、非有限数、负时间步长应拒绝，而非悄悄继续。
• 无新观测的时间段仍传播，过程噪声持续增加不确定性。

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
#include <stdexcept>

using V2 = std::array<double, 2>;
using M2 = std::array<V2, 2>;

struct KF2 {
    V2 x{0.0, 0.0};  // position [m], velocity [m/s]
    M2 P{{{1.0, 0.0}, {0.0, 1.0}}};

    // a: known navigation-frame acceleration for this interval.
    // q: continuous white acceleration-noise intensity [m^2/s^3].
    void predict(double dt, double a, double q) {
        if (!std::isfinite(dt) || dt < 0 || !std::isfinite(a) ||
            !std::isfinite(q) || q < 0)
            throw std::invalid_argument("bad prediction input");
        const M2 old = P;
        x[0] += x[1] * dt + 0.5 * a * dt * dt;
        x[1] += a * dt;
        P[0][0] = old[0][0] + dt*(old[0][1]+old[1][0])
                  + dt*dt*old[1][1] + q*dt*dt*dt/3.0;
        P[0][1] = old[0][1] + dt*old[1][1] + q*dt*dt/2.0;
        P[1][0] = old[1][0] + dt*old[1][1] + q*dt*dt/2.0;
        P[1][1] = old[1][1] + q*dt;
    }

    // h={1,0}: position; h={0,1}: velocity.
    // gate is an NIS threshold; gate<0 disables gating for this exercise.
    bool update(double z, double R, V2 h, double gate = -1.0) {
        if (!std::isfinite(z) || !std::isfinite(R) || R <= 0 ||
            !std::isfinite(h[0]) || !std::isfinite(h[1]) ||
            !std::isfinite(gate))
            throw std::invalid_argument("bad observation");
        const M2 old = P;
        V2 ph{old[0][0]*h[0]+old[0][1]*h[1],
              old[1][0]*h[0]+old[1][1]*h[1]};
        const double S = h[0]*ph[0] + h[1]*ph[1] + R;
        const double r = z - h[0]*x[0] - h[1]*x[1];
        if (!std::isfinite(S) || S <= 0 || !std::isfinite(r))
            throw std::runtime_error("invalid innovation covariance");
        if (gate >= 0 && r*r/S > gate) return false;
        V2 K{ph[0]/S, ph[1]/S};
        for (int i=0; i<2; ++i) x[i] += K[i]*r;
        // Joseph form: (I-KH) P (I-KH)^T + K R K^T.
        M2 A{}, out{};
        for (int i=0; i<2; ++i)
            for (int j=0; j<2; ++j)
                A[i][j] = (i==j ? 1.0 : 0.0) - K[i]*h[j];
        for (int i=0; i<2; ++i) {
            for (int j=0; j<2; ++j) {
                for (int k=0; k<2; ++k)
                    for (int l=0; l<2; ++l)
                        out[i][j] += A[i][k]*old[k][l]*A[j][l];
                out[i][j] += K[i]*R*K[j];
            }
        }
        P = out;
        P[0][1] = P[1][0] = 0.5*(out[0][1]+out[1][0]);
        return true;
    }
};

bool near(double a, double b) { return std::abs(a-b) < 1e-10; }

int main() {
    KF2 seed;
    seed.x = {10.0, 1.0};
    seed.P = M2{{{4.0, 1.0}, {1.0, 1.0}}};
    KF2 a = seed;
    assert(a.update(12.0, 1.0, {1.0, 0.0}));
    assert(near(a.x[0],11.6) && near(a.x[1],1.4));
    assert(near(a.P[0][0],0.8) && near(a.P[0][1],0.2));
    assert(near(a.P[1][0],0.2) && near(a.P[1][1],0.8));

    KF2 b=seed, c=seed;
    b.update(12.0,1.0,{1.0,0.0}); b.update(1.2,0.04,{0.0,1.0});
    c.update(1.2,0.04,{0.0,1.0}); c.update(12.0,1.0,{1.0,0.0});
    for(int i=0;i<2;++i) {
        assert(near(b.x[i],c.x[i]));
        for(int j=0;j<2;++j) assert(near(b.P[i][j],c.P[i][j]));
    }
    KF2 zero=seed;
    zero.update(10.0,1.0,{1.0,0.0});
    assert(near(zero.x[0],10.0) && zero.P[0][0]<seed.P[0][0]);
    KF2 reject=seed;
    assert(!reject.update(1000.0,1.0,{1.0,0.0},9.0));
    assert(reject.x==seed.x && reject.P==seed.P);
    KF2 propagation;
    propagation.predict(1.0,0.0,0.3);
    assert(near(propagation.P[0][0],2.1));
    assert(near(propagation.P[0][1],1.15));
    assert(near(propagation.P[1][1],1.3));
    bool caught=false;
    try { propagation.predict(-0.1,0,0); }
    catch(const std::invalid_argument&) { caught=true; }
    assert(caught);
    std::cout << "All fusion mechanism tests passed\n";
}

14. 面试时用这段逻辑回答，而不是只背增益公式

“我先定义统一的导航状态和误差协方差。惯导方案中，IMU 的去偏角速度与比力驱动高频传播；DVL 通过含坐标变换与杆臂的速度模型更新；USBL/GPS 通过位置观测模型更新。每个观测在自己的测量时刻与预测读数形成创新，通过创新协方差和卡尔曼增益修正整份状态，所以位置观测也能经交叉协方差修正速度和偏置。延迟观测需历史融合或回滚重放，失效则停止该观测更新并允许不确定性增长，不能重复旧值伪造新信息。最后检查可观测性、创新统计、恢复策略及控制输出的一致性。”