02.5｜失锁时估计器知道什么：可观测性、协方差与因子图延伸

本课把“有数值”和“有约束”分开。输入是传感器组合、运动轨迹及误差模型；输出是哪些状态能被区分、缺测时不确定度如何增长，以及恢复后为何不能无条件瞬时归零。假设先分析局部线性系统；实际非线性系统的可观测性会依赖运动，模型失配还会让计算出的协方差偏乐观。

符号／对象	含义与维度	单位与约定
$A,H,\mathcal O$	转移、观测矩阵、可观测性矩阵	局部线性模型，各块可有不同量纲
$c,\psi,R_z(\psi)$	水平平移、整体偏航、绕下轴旋转	m、rad、无量纲
$q_a,T$	加速度白噪声谱密度、断流时间	m²/s³、s
$P_{pp},P_{pv},P_{vv}$	位置方差、位置速度互协方差、速度方差	m²、m²/s、m²/s²
$X,r_i,\Sigma_i$	全窗口状态、因子残差、残差协方差	每项采用所属观测单位；白化平方无量纲

需要补的数学只读这些： 秩与独立信息和状态空间模型。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。

“能测到”与“能推出来”，不是同一个问题

直接可测问“传感器这一帧有没有这个量”；可观测问“给定已知模型和输入，一段输出历史能否区分不同初始状态”。速度没有独立传感器也可能从前后位置变化推出来；反过来，只有速度读数，再多历史也不一定知道绝对位置原点。

想象两艘艇始终同速同向，初始位置相差 $100\,\mathrm m$ 。如果唯一传感器只报告速度，它们产生完全相同的日志。任何算法都不能从相同输入数据可靠地选出两种不同答案。这个“找两个不同状态，却产生相同观测”的反例，比先背秩判据更能说明缺了什么信息。

秩是在数独立信息，不是在数读数条数。 同一条约束复制一百次仍只限制一个方向。下一节把 $H$ 与 $HA$ 堆起来，是把“现在看见什么”和“经过已知运动后看见什么”写成联立方程。满秩意味着理想模型里没有被完全隐藏的状态方向；不代表噪声很大、时间很短时也能高精度估计。可观测性、估计精度、当前传感器有效性是三个不同检查。

“观察器／观测器”是用输入、输出及模型重建状态的算法；“滤波器”强调怎样处理带噪数据和不确定度。卡尔曼滤波器也可以看作一种观测器，不是与观测器互斥的传感器种类。本课研究的信息限制同时约束它们：换算法名字不会创造缺失的绝对位置信息。

微练习： 没有外部位置锚点时，能否把初始水平位置设为零？能否据此宣称绝对位置已被观测？

概念答案：选原点不等于测原点

可以设零作为相对坐标约定；不能把约定说成测量证据。相对运动仍可有用，但绝对平移自由度没有消失。用匀速KF练习比较有位置观测的情况，再在自己的离线副本改成仅速度观测，检查位置不确定性不能凭空消失；不要修改练习输入协议。

先从一个能算秩的例子理解可观测性

1．只有速度时，为什么绝对位置偏移怎么也估不出来

先用二维状态 $x=[p,v]^T$ 分析，不让15维矩阵遮住本质。假设匀速模型，步长 $\Delta t$ ，则 $p_{k+1}=p_k+v_k\Delta t$ 、 $v_{k+1}=v_k$ ，写成

A=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix},\qquad H=\begin{bmatrix}0&1\end{bmatrix}.

$H$ 表示当前只测速度。下一步观测从当前状态看是 $z_{k+1}=HAx_k$ ；所以连续两次观测对当前状态的约束堆叠为

\mathcal O=\begin{bmatrix}H\\HA\end{bmatrix}=\begin{bmatrix}0&1\\0&1\end{bmatrix}.

两行相同，秩只有1：速度可以被区分，位置这一维不行。取状态 $[0,0.2]^T$ 与 $[100,0.2]^T$ ，无论向后预测多久，输出速度永远一样。滤波器可以选一个初始位置作为相对原点，却不能声称DVL观测证明了这个绝对原点。

如果改为测位置 $H=[1,0]$ ，两行会成为 $[1,0]$ 和 $[1,\Delta t]$ 。在 $\Delta t\ne0$ 时秩为2，因为两次位置差提供了速度信息。这一小例子说明可观测性要同时看运动模型和观测，不只看传感器名称。

对IMU、底跟踪DVL和压力组合，也能构造一种不变变换：把全部轨迹绕重力轴转一个固定角度，同时把姿态同样旋转，并给水平位置加常量。体轴速度与深度均不变。若没有磁航向、声学位置或外部已知地图方向，系统就无法单凭这些观测选出绝对北向和绝对水平原点。

2．三维系统的“不变变换”比盯着大矩阵更直观

对IMU、底跟踪DVL和压力系统，做一个思想实验：把完整估计轨迹绕NED下轴转固定角 $\psi$ ，再平移水平原点 $c$ 。设 $R_z(\psi)$ 为绕下轴的旋转， $c_z=0$ ，新状态定义为

p'=R_z(\psi)p+c,\qquad v'=R_z(\psi)v,\qquad R'=R_z(\psi)R.

这里 $p,v$ 在导航系， $R$ 是机体到导航系的姿态；我们同时改变导航描述，而不是让机器人真的动了一次。DVL的体轴速度预测变成

(R')^Tv'=R^TR_z(\psi)^TR_z(\psi)v=R^Tv.

中间两个旋转因正交性抵消，所以传感器输出不变。压力只看深度，设 $e_3=[0,0,1]^T$ ，由于绕竖直轴旋转不改变z分量，且平移没有z分量，得到

e_3^Tp'=e_3^Tp.

IMU方程中的重力方向在这个变换下也保持不变。因此这一整族不同的水平原点和绝对航向，都能产生相同数据。想固定它们，需要额外方向或位置锚点，而不是单纯延长运行时间。运动激励可以改善某些偏置估计，却不会打破这个整体坐标自由度。

失去DVL时，P为什么增长，而不是简单打一个无效标志

3．断流两秒，位置标准差为什么不是速度标准差乘两秒这么简单

考虑一维误差模型 $\dot p=v$ 、 $\dot v=w_a$ ， $w_a$ 为加速度白噪声，谱密度 $q_a$ 单位 $\mathrm{m^2/s^3}$ 。无噪声部分在 $T$ 秒后的转移为

\Phi(T)=\begin{bmatrix}1&T\\0&1\end{bmatrix}.

现在考虑时刻 $\tau$ 的一小份加速度扰动。它对最终速度的影响权重为1，对最终位置的影响权重为剩余时间 $T-\tau$ 。所以这一份噪声对状态的方向是 $[T-\tau,1]^T$ 。外积并对所有时刻积分：

Q_d(T)=q_a\int_0^T\begin{bmatrix}(T-\tau)^2&T-\tau\\T-\tau&1\end{bmatrix}d\tau =q_a\begin{bmatrix}T^3/3&T^2/2\\T^2/2&T\end{bmatrix}.

三个时间幂不是记忆规则：平方积分得到 $T^3/3$ ，一次积分得到 $T^2/2$ ，常数积分得到 $T$ 。同一噪声同时影响位置和速度，因此中间的互协方差不能删掉。

将 $P(T)=\Phi P(0)\Phi^T+Q_d$ 展开到位置元素，得到

P_{pp}(T)=P_{pp}(0)+2TP_{pv}(0)+T^2P_{vv}(0)+q_aT^3/3.

四项分别表示原位置不确定性、原位置与速度的关联、原速度误差被积分，以及新加速度噪声积累。速度方差则是 $P_{vv}(T)=P_{vv}(0)+q_aT$ 。

代入 $P_{pp}(0)=0.01$ 、 $P_{vv}(0)=0.04$ 、 $P_{pv}(0)=0$ 、 $q_a=0.01$ 、 $T=2$ （分别采用对应SI单位），位置方差为 $0.01+0.16+0.0266667=0.1966667\ \mathrm{m^2}$ ，标准差 $0.44347\ \mathrm m$ ；速度标准差 $\sqrt{0.06}=0.24495\ \mathrm{m/s}$ 。这就是断流期间仍必须传播 $P$ 的原因：均值可以看起来平滑，知识却在变少。

手算：初始位置方差0.01 m²、速度方差0.04 m²/s²、交叉项零， $q_a=0.01\,\mathrm{m^2/s^3}$ ，DVL缺失2 s，则位置方差0.196667 m²，标准差约0.4435 m；速度方差0.06，标准差约0.2449 m/s。压力仍可约束垂直分量，但不会直接补回水平速度。若另有未知恒定加速度偏置标准差0.02 m/s²，其位置标准差贡献约0.5×0.02×T²，2 s为0.04 m，10 s已达1 m；这与白噪声的 $T^3$ 方差律不是同一种误差机制。

退化与恢复：估计层和控制层各做什么

短缺测时继续用IMU传播名义量及 $P$ ，同时携带观测年龄和估计有效性。长缺测时，任务不能仅因位置数字连续就继续依赖精确悬停；应依据已授权模式的有效性逻辑降低对不可观测状态的依赖。恢复DVL先判断连续性和时间覆盖，再检查创新；重建增量基准后才能形成新观测。滤波层处理置信度，模式层决定哪些目标仍可执行，二者不能互相代替。

大创新可能来自真实状态漂移，也可能来自错轴、失底锁定或时间错配。若先验 $P$ 正确反映了断流增长， $S$ 也会增大，恢复时合理差异不一定被拒绝；若 $P$ 被错误夹得很小，系统会拒绝所有真实恢复观测。另一方面，盲目放大 $P$ 或解除所有门限会接纳错误数据。教学仿真应分别加入偏置、断流、错时间戳，每次只改一项，再观察创新与 $P$ 的物理解释。

先独立手写：从断流协方差看见失去的信息

根据初始 $2\times2$ 协方差和断流时间T计算 $P(T)$ ，完整保留互协方差。
改变T为1、2、5、10，区分原速度误差产生的 $T^2$ 项与新加速度噪声产生的 $T^3$ 项。
写三节点位移残差，给所有位置加同一个常数，检查代价是否变化；再加入首点先验。
说明为什么有压力观测并不意味着水平绝对位置也可观测。

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

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

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
int main(){
    // 状态[p,v]；所有数值采用SI单位。
    const double T=2,qa=.01,p00=.01,p01=0,p11=.04;
    const double Ppp=p00+2*T*p01+T*T*p11+qa*T*T*T/3;
    const double Ppv=p01+T*p11+qa*T*T/2;
    const double Pvv=p11+qa*T;
    assert(Ppp>0 && Pvv>0 && Ppp*Pvv-Ppv*Ppv>0);
    assert(std::abs(Ppp-.19666666666666667)<1e-12);
    std::cout<<std::sqrt(Ppp)<<' '<<std::sqrt(Pvv)<<'\n';
    // 因子图平移零空间：只约束相邻位移，平移所有节点不改残差。
    std::array<double,3> a{0,1,2},b{10,11,12};
    auto cost=[](const auto&p){double r1=p[1]-p[0]-1,r2=p[2]-p[1]-1;return r1*r1+r2*r2;};
    assert(cost(a)==0 && cost(b)==0);
    // 加入p0=0的单位方差先验后，第二条轨迹的代价增加100。
    assert(cost(b)+b[0]*b[0]==100);
}

因子图不是另一种传感器，而是保留更多时刻的联合估计

KF用当前均值和协方差概括历史；因子图把多个时刻的状态作为变量，把IMU区间、DVL速度、压力深度与先验写成连接这些变量的概率因子。假设各因子条件独立且高斯，对后验概率取负对数，乘积就变为加权残差平方和：

4．因子图从概率乘积变成最小二乘，而不是另一个滤波黑盒

若希望迟到观测能够修正过去多个时刻，可以把窗口内状态全部保留，记为 $X$ 。每条观测或运动关系产生残差 $r_i(X)$ ；在条件独立、高斯噪声的假设下，该因子的概率与 $\exp[-\tfrac12r_i^T\Sigma_i^{-1}r_i]$ 成正比。 $\Sigma_i$ 是对应残差的协方差，不同因子可有不同尺寸和物理单位。

概率连乘后取负对数，指数的乘积变为和，忽略与 $X$ 无关的常数，最大后验问题变成

X^*=\arg\min_X\frac12\sum_i r_i(X)^T\Sigma_i^{-1}r_i(X).

每一项都是无量纲的归一化误差平方；不能把m误差、rad误差直接相加。非线性时，在当前轨迹附近加一个局部增量 $\delta$ 。符号 $\boxplus$ 表示位置等向量相加、姿态采用局部旋转组合：

r_i(X\boxplus\delta)\approx r_i(X)+J_i\delta.

把它代回目标函数，再对 $\delta$ 求导：每项贡献 $J_i^T\Sigma_i^{-1}(r_i+J_i\delta)$ 。令总导数为零并把常数项移到右侧，就得到正规方程

\left(\sum_iJ_i^T\Sigma_i^{-1}J_i\right)\delta =-\sum_iJ_i^T\Sigma_i^{-1}r_i.

若本章仍用 $r=z-h$ ，则 $J=-H$ ；若改用 $h-z$ ，残差和导数同时变号，目标不变。解出增量后更新轨迹，再重新线性化，才是一个完整迭代步骤。它与KF都在利用同样的观测物理关系，区别主要在保留多少时刻、何时重新线性化和如何消去历史变量。

三节点手算：位移因子要求 $p_1-p_0=1$ 、 $p_2-p_1=1$ 。 $[0,1,2]$ 与 $[10,11,12]$ 的残差都为零，所以没有首点先验时正规矩阵存在平移零空间。加上 $p_0=0$ 的先验才固定原点。换成图优化不会凭空创造绝对位置信息。

最小思想实验：三个时刻的标量位置，两个位移约束各为1 m；没有先验时， $[0,1,2]$ 与 $[10,11,12]$ 同样好，正规矩阵必有平移零空间。加入首点0 m先验才固定原点。图优化不会自动消除不可观测性。固定滞后窗口删除旧节点时，要把它们的信息通过边缘化保留为新先验；这不是把旧因子直接丢掉。GTSAM官方教程区分因子图与Values，并用先验、里程计和非线性优化展示这种建模方式。TB02-S3

ArduSub对应位置与综合手写作业

补充练习答案与阅读提示

本项目链路用延迟EKF与当前输出预测器服务控制；因子图在这里是离线重处理与后续多源融合的学习扩展，不改变项目现有反馈路线。源码阅读把三层画开：DVLVelocityFilter负责观测连续性，UpdateFilter负责状态约束，AP_InertialNav_NavEKF::update把估计转换为控制接口。练习：完成一维 $x=[p,v,b_a]^T$ 滤波，100 Hz IMU预测、10 Hz速度观测，中途2 s断流；对比不传播 $P$ 、正常传播 $P$ 、错误重复融合旧DVL三种情况。答案要点：只有正常传播 $P$ 如实反映失锁增长；重复旧观测会虚假压缩 $P$ 且给状态施加旧速度；不传播 $P$ 会使恢复门限与增益失真。再加压力只应改变垂直子问题，不能拿它补水平绝对位置。

完成本章的判断标准不是记住EKF缩写，而是能从原始字节一路说明：这是谁在什么时候、哪个坐标系、用什么单位测到的量；它对应哪个h与 $H$ ；预测误差怎样经 $F$ 传播； $R$ 与 $Q$ 分别来自哪里；更新后为什么注入、重置、再预测到当前时刻。精读因子图时先手算三节点链的平移零空间，再看GTSAM教程的先验与里程计示例，不必一开始就依赖大型优化库。

TB02-S3 · Frank Dellaert／GTSAM｜Factor Graphs: A Hands-on Introduction

精读因子图与Values的区别、先验和里程计因子、非线性优化；本课三节点与正规方程为教学推演。