10_贝叶斯滤波与线性高斯滤波
博士前数学路线图·第五批·子专题 A1：贝叶斯滤波框架与线性高斯滤波族——KF、Information Filter、Square Root Filters 与一致性诊断¶
底线陈述（BLUF）：本章把后续 5-A2/A3/A4/5-B 反复引用的"地基"一次性讲透——所有 Kalman 族滤波器都只是"贝叶斯递推 + 高斯假设"在不同代数形式下的投影。线性高斯 KF 是最优估计（BLUE + MMSE + MAP 三合一；MMSE/MAP 等价性要求联合高斯，线性模型下自动成立），其协方差形式、信息形式、平方根形式在代数上等价但数值性态迥异：协方差形易懂却脆弱，信息形天然支持多传感器融合与稀疏 SLAM 后端，平方根形是 Apollo 登月以来航天/GNSS 工业界的数值黄金标准。而 NEES/NIS/\(\chi^2\) gate 这套一致性诊断是判断"你的滤波器是否真的工作"的唯一严肃方法，也是 MSCKF/OpenVINS 等现代 VIO 的标配。掌握本章后，EKF/UKF（5-A2）是把线性化算子插进去，ESKF/InEKF（5-A3）是把代数空间换到流形上，RTS/MHE（5-A4）是把前向递推扩展到前向-后向或滑动窗口，因子图（5-B）是把同一信息矩阵重新组织成图结构。本章按"贝叶斯框架→KF→信息形→平方根→一致性→工程"六段式展开，兼顾严格推导、伪代码、陷阱清单与代码库映射。
目标档位：档位 3（博士入学）20–25h；档位 4（进阶）再加 5–8h。 读者假设：熟悉线性代数（含 Cholesky/QR/Woodbury）、多元高斯、基本随机过程；有 C++/Python 工程经验。
前置自测 ⭐¶
📋 答不出 >= 2 题 → 先回前置章节复习
| 编号 | 问题 | 答不出时回顾 |
|---|---|---|
| 1 | 写出多元高斯分布的密度函数 \(p(x)=\mathcal{N}(\mu,\Sigma)\)，指出指数中的二次型 \((x-\mu)^\top\Sigma^{-1}(x-\mu)\) 的几何含义（等高面是什么形状？） | 线性代数·多元高斯 |
| 2 | 什么是 Cholesky 分解？\(P=LL^\top\) 中 \(L\) 满足什么结构？什么条件下 Cholesky 分解存在？ | 线性代数·矩阵分解 |
| 3 | Woodbury 矩阵恒等式 \((A+BCD)^{-1}=?\)。给出完整公式并说明 \(A,C\) 需满足什么条件。 | 线性代数·矩阵求逆 |
| 4 | 给定 \(x\sim\mathcal{N}(\mu_x,\Sigma_x)\)，线性变换 \(y=Ax+b\) 后，\(y\) 的均值和协方差分别是什么？ | 概率论·高斯变换 |
| 5 | 什么是条件概率 \(p(x\mid y)\)？写出 Bayes 公式。 | 概率论基础 |
本章目标¶
学完本章后，你应该能够：
- **推导**贝叶斯滤波的预测–更新递推框架，理解 Chapman–Kolmogorov 和 Bayes 规则在其中的角色
- **手推**线性 KF 的全部公式（含 Joseph form），并解释 Kalman 增益的物理意义
- **对比**协方差形、信息形、平方根形三种 KF 实现的优劣与适用场景
- 使用 NEES/NIS/\(\chi^2\) gate 诊断滤波器一致性，判定"过自信"还是"过保守"
- **实现**一个最小线性 KF（C++ 或 Python），含 Joseph form 和 NIS gate
§A1.1 状态估计的概率视角 ⭐⭐¶
动机。机器人感知到的是**含噪观测序列** \(y_{1:T}\)，需要推断**隐状态** \(x_{0:T}\)（位姿、速度、偏置、地图等）。概率视角把一切统一为后验 \(p(x_{0:T}\mid y_{1:T})\)——这不仅给出点估计，还给出不确定性。
状态空间模型（State-Space Model / Hidden Markov Model）。一般形式（Särkkä & Svensson 2ed §4.1；Barfoot 2ed §3.1.1）： $\(x_k = f(x_{k-1}, u_{k-1}, w_{k-1}), \qquad y_k = h(x_k, v_k)\)$ 对应**转移密度** \(p(x_k\mid x_{k-1}, u_{k-1})\) 与**观测似然** \(p(y_k\mid x_k)\)。
一阶马尔可夫假设： $\(p(x_k\mid x_{0:k-1}, y_{1:k-1}) = p(x_k\mid x_{k-1}, u_{k-1})\)$ 观测条件独立：\(p(y_k\mid x_{0:k}, y_{1:k-1}) = p(y_k\mid x_k)\)。
这两条假设把 \(p(x_{0:T}\mid y_{1:T})\) 因子化为可递推计算的形式，是**所有在线滤波器**的根基。
三类推断任务。读者必须区分：
| 任务 | 目标分布 | 因果性 | 典型场景 |
|---|---|---|---|
| 滤波 (filtering) | \(p(x_k\mid y_{1:k})\) | 在线、因果 | 实时状态反馈、VIO 前端 |
| 预测 (prediction) | \(p(x_{k+n}\mid y_{1:k}),\, n\ge 1\) | 因果外推 | MPC、轨迹预报 |
| 平滑 (smoothing) | \(p(x_k\mid y_{1:T}),\, k<T\) | 离线、非因果 | 建图后处理、RTS、MHE |
本章专注**滤波**；平滑将在 5-A4 展开。
§A1.2 预测–更新两步递推（Chapman–Kolmogorov + Bayes） ⭐⭐¶
核心思想。假设 \(t=k-1\) 时已持有后验 \(p(x_{k-1}\mid y_{1:k-1})\)，**如何一步推进**到 \(p(x_k\mid y_{1:k})\)？答案是经典的"预测–更新"双子步（Särkkä 2ed Thm 4.1 "Bayesian optimal filtering equations"）。
预测步（Chapman–Kolmogorov 方程）。对隐状态时间传播： $\(\boxed{\,p(x_k\mid y_{1:k-1}) = \int p(x_k\mid x_{k-1})\,p(x_{k-1}\mid y_{1:k-1})\,dx_{k-1}\,}\)$ 推导仅用马尔可夫性 + 边缘化：把联合 \(p(x_k, x_{k-1}\mid y_{1:k-1})\) 写成 \(p(x_k\mid x_{k-1})p(x_{k-1}\mid y_{1:k-1})\)，对 \(x_{k-1}\) 积分即得。
更新步（Bayes 规则）。新量测 \(y_k\) 到来时： $\(\boxed{\,p(x_k\mid y_{1:k}) = \frac{p(y_k\mid x_k)\,p(x_k\mid y_{1:k-1})}{Z_k},\quad Z_k = \int p(y_k\mid x_k)\,p(x_k\mid y_{1:k-1})\,dx_k\,}\)$ \(Z_k\) 是**归一化常数**，也叫**新息似然** (innovation likelihood)，常用于模型选择、异常检测、期望最大化参数辨识。
递推结构图： $\(p(x_{k-1}\mid y_{1:k-1})\ \xrightarrow{\text{Chapman-Kolmogorov}}\ p(x_k\mid y_{1:k-1})\ \xrightarrow{\text{Bayes, 乘 }p(y_k\mid x_k)\text{ 归一}}\ p(x_k\mid y_{1:k})\)$
为什么高斯 + 线性 = 闭式解？ 因为高斯族在**线性变换下闭合**（预测步积分可解析得出）、在**乘积下闭合**（更新步两个高斯的乘积仍是高斯）。任意一条被破坏就需要近似——非线性动力学催生 EKF/UKF（5-A2），流形结构催生 ESKF/InEKF（5-A3），非高斯则催生粒子滤波（本章不涉及）。
贝叶斯滤波预测–更新总结表¶
| 步骤 | 输入 | 运算 | 输出 | 线性高斯情形退化为 |
|---|---|---|---|---|
| 预测 | \(p(x_{k-1}\mid y_{1:k-1})\) + 动力学 \(p(x_k\mid x_{k-1})\) | Chapman–Kolmogorov 积分 | 先验 \(p(x_k\mid y_{1:k-1})\) | KF 预测：\(\hat x^-=F\hat x+Bu,\ P^-=FPF^\top+Q\) |
| 更新 | 先验 + 似然 \(p(y_k\mid x_k)\) | Bayes 乘法 + 归一化 | 后验 \(p(x_k\mid y_{1:k})\) | KF 更新：\(K=P^-H^\top S^{-1},\ \hat x^+=\hat x^-+K\tilde y\) |
§A1.3 贝叶斯最优估计的性质 ⭐⭐¶
三种点估计准则——MMSE、MAP、BLUE——在高斯情形下三合一，这也是 KF 之所以"最优"的深层原因。精确条件：MMSE 与 MAP 的等价性要求**状态与观测的联合高斯**（而非仅边际高斯）。在线性高斯系统 \(x_k = Fx_{k-1}+w\)、\(y_k=Hx_k+v\) 下，联合高斯由线性结构自动成立，因此 KF 语境中该条件无需额外验证。
MMSE（Minimum Mean Square Error）： $\(\hat x_{\text{MMSE}} = \arg\min_{\hat x}\mathbb{E}[\|x-\hat x\|^2\mid y] = \mathbb{E}[x\mid y]\)$ 证明思路：展开 \(\mathbb{E}[\|x-\hat x\|^2\mid y]\) 对 \(\hat x\) 求导为零。无论后验是什么分布，MMSE 总是后验均值。
MAP（Maximum A Posteriori）： $\(\hat x_{\text{MAP}} = \arg\max_x p(x\mid y)\)$ 对高斯后验：均值 = 众数，MAP = MMSE。对非高斯（如双峰）：二者可相差很大——这是后续因子图 MAP 求解（5-B）与滤波 MMSE 之间细微差别的源头。
BLUE（Best Linear Unbiased Estimator）：在所有形如 \(\hat x = a + By\) 的**线性无偏**估计中，使均方误差最小者。Gauss–Markov 定理保证：线性高斯系统下 KF 即 BLUE，即便放弃高斯假设也成立（只需二阶矩已知）。这点极重要：KF 在"协方差已知"的更弱条件下仍是最优线性估计器，这也是为什么它能处理 IMU 白噪建模而无需假设真正高斯。
§A1.4 线性高斯系统定义 ⭐⭐¶
标准形式： $\(x_k = F_{k-1}x_{k-1} + B_{k-1}u_{k-1} + w_{k-1},\qquad w_{k-1}\sim\mathcal{N}(0, Q_{k-1})\)$ $\(y_k = H_kx_k + v_k,\qquad v_k\sim\mathcal{N}(0, R_k)\)$ $\(x_0\sim\mathcal{N}(\hat x_0, P_0)\)$
白噪声假设：\(w_k, v_k\) 零均值、同时刻独立（\(\mathbb{E}[w_kv_k^\top]=0\)）、序列不相关（\(\mathbb{E}[w_kw_j^\top]=Q_k\delta_{kj}\)），与初始状态 \(x_0\) 独立。
关键隐含： - \(Q\succeq0\) 可为半正定；协方差形更新中通常要求 \(R\succ0\)，因为需要求 \(S^{-1}\)。奇异 \(R\)（无噪量测）需改写为约束、信息形或 UD/平方根形式处理。 - \(F\) 可任意（不必可逆；信息形预测的直接公式只需 \(Q\succ 0\)，但使用 Woodbury 加速形式时需 \(F\) 可逆）。 - 离散化：若底层是连续 SDE \(\dot x = A x + B u + L \eta\)，离散 \(F = e^{A\Delta t}\)、\(Q_d = \int_0^{\Delta t} e^{A\tau}L\Sigma L^\top e^{A^\top\tau}d\tau\)（Van Loan 算法）；工程上经常 \(Q_d \approx \Sigma\Delta t\) 一阶近似，见 Simon §5.5。
§A1.5 KF 的贝叶斯推导（完成平方法） ⭐⭐¶
推导动机：把 §A1.2 的通用贝叶斯递推落到线性高斯情形，闭式得出 KF 公式，让读者看清"递推解"从何而来。
预测步（高斯 × 线性变换 = 高斯，直接套矩匹配）： $\(\hat x_{k|k-1} = F_{k-1}\hat x_{k-1|k-1} + B_{k-1}u_{k-1}\)$ $\(P_{k|k-1} = F_{k-1}P_{k-1|k-1}F_{k-1}^\top + Q_{k-1}\)$
更新步（配方法 / completion of squares）。展开 \(-2\ln p(x_k\mid y_{1:k})\) 的 \(x_k\) 二次型： $\(-2\ln p(x_k\mid y_{1:k}) = (x_k-\hat x^-)^\top (P^-)^{-1}(x_k-\hat x^-) + (y_k-Hx_k)^\top R^{-1}(y_k-Hx_k) + \text{const}\)$ 收集 \(x_k\) 的二次项与一次项、配成 \((x_k-\mu)^\top\Sigma^{-1}(x_k-\mu)\) 形式，直接读出： $\(\boxed{\,\Sigma^{-1} = P^{-1} + H^\top R^{-1}H,\qquad \Sigma^{-1}\mu = P^{-1}\hat x^- + H^\top R^{-1}y_k\,}\)$ 这就是 信息形 KF 更新！对它做 Woodbury（§A1.7），立刻变回协方差形：
协方差形更新（innovation form）： $\(\tilde y_k = y_k - H_k\hat x_{k|k-1},\quad S_k = H_kP_{k|k-1}H_k^\top + R_k\)$ $\(K_k = P_{k|k-1}H_k^\top S_k^{-1}\)$ $\(\hat x_{k|k} = \hat x_{k|k-1} + K_k\tilde y_k\)$ $\(P_{k|k} = (I - K_kH_k)P_{k|k-1}\qquad\text{(简化形)}\)$
Joseph form（数值稳定版，精确算术下是两个 PSD 矩之和；在有限精度下也比简化形显著更不容易破坏半正定性）： $\(\boxed{\,P_{k|k} = (I-K_kH_k)P_{k|k-1}(I-K_kH_k)^\top + K_kR_kK_k^\top\,}\)$ 源于 Bucy–Joseph 1968，Apollo 导航计算机因字长短首次强制要求。
线性 KF 完整伪代码¶
# ---- 初始化 ----
x = x0; P = P0
# ---- 滤波循环 ----
for k in 1..T:
# 预测：用系统模型传播均值与协方差
x = F @ x + B @ u[k-1]
P = F @ P @ F.T + Q
# 更新：如果当前时刻没有量测，则跳过这一段
y_tilde = z[k] - H @ x
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.solve(S, np.eye(m)).T # 或 solve(S, H@P).T
x = x + K @ y_tilde
I_KH = I - K @ H
P = I_KH @ P @ I_KH.T + K @ R @ K.T # Joseph form
直觉。\(K_k\) 是"新息权重"：\(R\) 小（量测可信）⇒ \(K\) 大，追量测；\(P^-\) 小（先验可信）⇒ \(K\) 小，保持先验。\(S_k\) 是新息协方差——新息归一化就是 NIS（§A1.16）。
如果不使用 Joseph form 会怎样？ 简化式 \(P_{k|k}=(I-K_kH_k)P_{k|k-1}\) 在精确算术下等价于 Joseph form，但浮点运算中减法会导致**灾难性消相抵**——\(P\) 可能失去对称性甚至失去半正定性。一旦 \(P\) 的某个特征值变负，下一步 Cholesky 分解直接崩溃，整个滤波器停机。Apollo 导航计算机（15-bit 定点）正是因此故障才催生了 Potter 的平方根滤波。工程教训：任何生产级 KF 都必须默认使用 Joseph form。
本质洞察：Kalman 增益 \(K\) 不是一个"经验调参"——它是先验不确定性 \(P^-\) 与观测不确定性 \(R\) 的精确博弈结果。当 \(R\to\infty\) 时 \(K\to 0\)（完全不信量测），当 \(P^-\to\infty\) 时 \(K\to H^\dagger\)（完全信量测）。
⚠️ 概念误区：认为 KF 必须假设高斯分布才有意义。 实际上 KF 作为 BLUE（下一节证明）只需要二阶矩已知——即便噪声不是高斯，KF 仍然是所有线性无偏估计中最优的。高斯假设的额外好处是让 KF 同时等于 MMSE 和 MAP，但这不是 KF 工程可用性的唯一基础。
练习¶
- 对标量系统 \(F=1,\,H=1,\,Q=0.1,\,R=1,\,P_0=10,\,\hat x_0=0\)，手算前 3 步的 \(P_{k|k-1},K_k,P_{k|k}\)，观察 \(P\) 的单调递减趋势。
- 把上题的 \(R\) 改成 \(0.01\)（高精度量测），重算前 3 步，解释为什么 \(K\) 变大、\(P\) 收敛更快。
- 写出 Joseph form 展开后在最优 \(K\) 下退化为简化形的证明步骤。
§A1.6 KF 作为 BLUE 的证明 ⭐⭐⭐¶
思路：在形如 \(\hat x_{k|k} = \hat x_{k|k-1} + K(y_k - H\hat x_{k|k-1})\) 的**所有线性无偏**估计中，找使 \(\mathrm{tr}(P_{k|k})\) 最小的 \(K\)。
关键等式（对任意 \(K\)）： $\(P_{k|k}(K) = (I-KH)P^-(I-KH)^\top + KRK^\top\)$ （这就是为什么 Joseph form 对**任意** \(K\) 都正确，而简化形只对最优 \(K\) 成立。）
意义：这个证明**不需要高斯假设**，只需 \(w,v\) 零均值 + 协方差已知。所以 KF 在"最优线性 MSE 估计"意义下对任意二阶矩分布都最优——这是 KF 在工程上被广泛信任的核心理由。Simon §5.3、Bar-Shalom §5.2 给出同样推导。**Gauss–Markov 定理**是此结果的静态版本。
§A1.7 矩阵恒等式工具箱——Woodbury 及其衍生 ⭐⭐¶
Matrix Inversion Lemma（Woodbury 恒等式）： $\((A + BCD)^{-1} = A^{-1} - A^{-1}B(C^{-1} + DA^{-1}B)^{-1}DA^{-1}\)$ 要求 \(A,C\) 可逆。
应用 1：协方差形 ⇄ 信息形。令 \(A = P^{-1},\, B = H^\top,\, C = R^{-1},\, D = H\)： $\((P^{-1} + H^\top R^{-1}H)^{-1} = P - PH^\top(HPH^\top + R)^{-1}HP\)$ 左边 = 信息形 \(P_{k|k}\)；右边 = 协方差形 \(P_{k|k}\)——两种 KF 形式是精确对偶，仅数值路径不同。
应用 2：Kalman gain 的等价形式： $\(\boxed{\,K = P^-H^\top(HP^-H^\top + R)^{-1} = (P^{-1}+H^\top R^{-1}H)^{-1}H^\top R^{-1}\,}\)$ 何时用哪个？ \(\dim(y)\ll\dim(x)\)（稀疏观测、VIO）→ 前者便宜（逆 \(m\times m\) 的 \(S\)）；\(\dim(y)\gg\dim(x)\)（高维观测批量融合、多 IMU 阵列）→ 后者便宜（逆 \(n\times n\)）。
应用 3：Sherman–Morrison（Woodbury 的 rank-1 特例）： $\((A + uv^\top)^{-1} = A^{-1} - \frac{A^{-1}uv^\top A^{-1}}{1 + v^\top A^{-1}u}\)$ **标量量测**逐个处理时用此式得 \(O(n^2)\) 更新（sequential KF，Simon §6.3），这也是 Bierman UD 更新的代数基础。
§A1.8 稳态 KF 与 Discrete Algebraic Riccati Equation (DARE) ⭐⭐⭐¶
观察：时不变系统 \((F, H, Q, R)\) 下 \(P_{k|k-1}\) 的递推**与量测无关**（预计算！），可离线收敛到稳态 \(P_\infty\)。
DARE： $\(\boxed{\,P = FPF^\top + Q - FPH^\top(HPH^\top + R)^{-1}HPF^\top\,}\)$ 稳态增益 \(K_\infty = P_\infty H^\top(HP_\infty H^\top + R)^{-1}\) 是**常数矩阵**，嵌入式系统可烧写成查表滤波器，省去每步矩阵求逆——航天器姿态控制、车辆 ESP、电机观测器都用此路数。
存在与唯一性条件： - 可检测 (detectable)：\((F, H)\)——所有不稳定模式至少被某量测观察到 ⇒ \(P_\infty\) 有界。 - 可稳 (stabilizable)：\((F, Q^{1/2})\)——过程噪声激励所有不稳定模式 ⇒ PSD 解唯一、闭环 \(F - K_\infty H\) 的所有特征值位于单位圆内（Schur）。
与 LQR 的对偶（Kalman–Bucy 1961）：
| Kalman Filter（LQE） | LQR |
|---|---|
| \(P=FPF^\top+Q-FPH^\top(HPH^\top+R)^{-1}HPF^\top\) | \(S=F^\top SF+Q_c-F^\top SB(R_c+B^\top SB)^{-1}B^\top SF\) |
| \((F,H)\) 可检测 | \((F,B)\) 可稳 |
| \((F,Q^{1/2})\) 可稳 | \((F,Q_c^{1/2})\) 可检测 |
| 前向时间递推 | 反向时间递推 |
Separation Principle。对 LQG 问题（线性系统 + 二次代价 + 高斯噪声）：最优控制 = LQR 最优增益 \(L^\star\) 作用在 KF 最优估计 \(\hat x_{k|k}\) 上——两者 Riccati 方程**独立设计互不耦合**。这就是**Certainty Equivalence（确定性等价）。**警告：该原理对非线性、非高斯、鲁棒设计**一般不成立**。
如果不满足可检测性会怎样？ 考虑一个不可检测的模式——状态中某个分量既不被观测到，又不受过程噪声激励（例如忘记把 IMU bias 加入状态但 bias 确实在漂移）。此时 \(P_\infty\) 沿该方向发散到无穷，稳态 KF 无法给出有界估计。工程上的直接表现是：滤波器"看起来跑着"，但某些状态分量的协方差单调增长，最终导致数值溢出或增益矩阵病态。教训：忘记建模 bias 不仅是精度问题，而是稳定性问题。
练习¶
- 对一维系统 \(F=1,H=1,Q=0.5,R=1\)，手解 DARE \(P=FPF^\top+Q-FPH^\top(HPH^\top+R)^{-1}HPF^\top\)（这是一个一元二次方程），验证稳态增益 \(K_\infty\)。
- 写出 LQR 的 DARE，指出与 KF DARE 之间 \(F\leftrightarrow F^\top\)、\(H\leftrightarrow B\) 的对偶关系。
- 举一个 Separation Principle 不成立的非线性系统例子（如角度状态的 \(\sin\theta\) 动力学），说明为什么估计和控制不能独立设计。
§A1.8b 连续–离散 Kalman 滤波（CD-KF） ⭐⭐⭐¶
这一节解决什么问题：真实机器人的动力学往往是连续时间 ODE（IMU 积分、电机模型），但传感器量测（GPS、相机帧、编码器脉冲）在离散时刻到达。CD-KF 把连续预测和离散更新无缝衔接，是 VIO/INS 工业实现中最常见的滤波架构。
动机¶
§A1.4 定义的全离散 KF 假设 \(x_k = F_{k-1}x_{k-1}+w_{k-1}\)——\(F\) 本身从何而来？如果底层物理是连续时间线性系统
量测仅在离散时刻 \(t_k\) 可用：
那么就面临两个子问题：
- 预测：在量测间隔 \([t_{k-1}, t_k]\) 内沿连续 ODE 传播均值和协方差。
- 更新：在离散时刻用标准 KF 公式吸收量测。
这就是**连续–离散 Kalman 滤波器（Continuous-Discrete KF, CD-KF）**的结构。OpenVINS 的 Propagator 用 RK4 积分 IMU 误差态 ODE 正是此模式；robot_localization 的 predict() 在高频 IMU 步间做类似离散化；PX4 EKF2 的 covariance_prediction() 也遵循同一原理。
连续预测：矩阵 Riccati ODE¶
在两次量测之间，均值和协方差满足：
这是**连续 Lyapunov/Riccati ODE**。注意这里没有更新项（因为量测还没来）。\(P(t)\) 在量测间隔内单调不减（所有特征值不减），物理直觉很清晰：没有新信息注入，不确定性只能增长或持平。
类比：如果把协方差想象成一团墨水在流场中的扩散，\(A_c\) 决定墨水团的拉伸和旋转方向，\(LQ_cL^\top\) 是扩散速率。量测就像在特定时刻"对焦"——把墨水团在某些方向压缩回来。
离散化：Van Loan 方法¶
工程上不直接解 Riccati ODE，而是把连续系统一次性离散化为 \((F_d, Q_d)\)，然后用全离散 KF 的预测公式。Van Loan (1978) 提供了一个优雅的精确方法：构造 \(2n\times 2n\) 增广矩阵
计算矩阵指数
从右下角读出 \(F_d^\top\)（转置得 \(F_d\)），从右上角读出 \(F_d^{-1}Q_d\)（左乘 \(F_d\) 得 \(Q_d\)）。
显式公式：
Van Loan 的增广矩阵方法把这个积分转化为一次矩阵指数计算，避免了数值积分。
参考：Van Loan, "Computing Integrals Involving the Matrix Exponential," IEEE TAC 23(3):395–404, 1978; Simon §5.5; Särkkä & Svensson 2ed §4.5。
一阶近似与精确解的对比¶
工程中经常看到两种简化：
| 近似级别 | \(F_d\) | \(Q_d\) | 适用条件 |
|---|---|---|---|
| 零阶 | \(I + A_c\Delta t\) | \(L\,Q_c\,L^\top\,\Delta t\) | \(\|A_c\|\Delta t \ll 1\) |
| 一阶 | \(I + A_c\Delta t + \frac{1}{2}(A_c\Delta t)^2\) | 含 \(\Delta t\) 和 \(\Delta t^2\) 项 | 中等步长 |
| 精确 | \(e^{A_c\Delta t}\) | Van Loan 积分 | 任何步长 |
如果用零阶近似但步长较大会怎样？ 例如 IMU 姿态传播中 \(A_c\) 包含角速度 \(\omega\)。若 \(\omega=2\pi\) rad/s（一秒转一圈）且 \(\Delta t = 0.01\) s，则 \(\|A_c\|\Delta t \approx 0.063\)，零阶近似误差约 \(0.2\%\)。但若 IMU 步长降到 \(\Delta t = 0.1\) s（低频采样），\(\|A_c\|\Delta t \approx 0.63\)，零阶误差可达 \(20\%\)——协方差传播会显著失准，NIS 诊断会报告过自信或过保守。
何时用 CD-KF vs 全离散 KF¶
| 场景 | 推荐 | 理由 |
|---|---|---|
| IMU（连续动力学）+ 低频 GPS/Camera | CD-KF | 动力学本身是连续 ODE，离散化是工程选择 |
| 纯离散传感器链（码盘脉冲→位置差分） | 全离散 KF | 动力学本身已是离散递推 |
| 高频 IMU + 高频量测（相同频率） | 全离散 KF | 每步都有量测，连续积分无意义 |
| 变步长量测（时间戳不均匀） | CD-KF | \(\Delta t\) 每步不同，\(F_d(\Delta t)\) 和 \(Q_d(\Delta t)\) 须按实际间隔重新计算 |
与 VIO 的连接：OpenVINS 的 Propagator::propagate_and_clone() 在每两个相机帧之间对所有 IMU 样本做逐步积分。每一小步的 \(\Delta t\) 是 IMU 时间戳间隔（通常 \(\sim\)5 ms），状态转移矩阵通过连续误差态 ODE 的离散化得到。这正是 CD-KF 的工程实现。
本质洞察：CD-KF 不是一种"新的滤波器"——它是把连续动力学和离散量测组合起来的**架构模式**。预测步走连续 Riccati ODE（或其离散化），更新步仍是标准 KF。所有后续的 EKF、UKF、ESKF 都可以在这个架构下运行。
⚠️ 概念误区：混淆连续噪声强度 \(Q_c\) 与离散噪声协方差 \(Q_d\) 的单位。 \(Q_c\) 的单位是 \([\text{state}^2/\text{time}]\)（功率谱密度），\(Q_d\) 的单位是 \([\text{state}^2]\)（方差）。二者的关系在最简单情况下是 \(Q_d \approx Q_c\Delta t\)，但这只在 \(A_c=0\) 时精确成立。工程中最常见的 bug 是：从 IMU 数据手册读到噪声密度（如 \(0.01\,\text{rad/s}/\sqrt{\text{Hz}}\)，这是 \(\sqrt{Q_c}\) 的元素），直接当作 \(\sqrt{Q_d}\) 用——少乘了 \(\sqrt{\Delta t}\)，导致过程噪声偏小约一个数量级，滤波器过自信。
练习¶
- 对二维常速度系统 \(A_c=\begin{bmatrix}0&1\\0&0\end{bmatrix}\)，\(L=\begin{bmatrix}0\\1\end{bmatrix}\)，\(Q_c=\sigma_a^2\)（标量加速度谱密度），用 Van Loan 方法构造 \(4\times4\) 增广矩阵，计算 \(e^{\mathcal{M}\Delta t}\)，读出 \(F_d\) 和 \(Q_d\)，验证 \(Q_d = \sigma_a^2\begin{bmatrix}\Delta t^3/3 & \Delta t^2/2\\\Delta t^2/2 & \Delta t\end{bmatrix}\)。
- 若 IMU 陀螺仪噪声密度为 \(0.004\,\text{rad/s}/\sqrt{\text{Hz}}\)，采样率 200 Hz（\(\Delta t=0.005\) s），计算离散角速度噪声标准差 \(\sigma_{\omega,d}\)。解释为什么直接把 \(0.004\) 当 \(\sigma_{\omega,d}\) 会让协方差偏小。
§A1.8c 自适应 Kalman 滤波（Q/R 在线估计） ⭐⭐⭐¶
这一节解决什么问题：标准 KF 假设 \(Q\) 和 \(R\) 精确已知。现实中它们几乎从来都是猜的或从数据手册粗略读取的。自适应 KF 试图从滤波过程本身在线估计 \(Q\) 和/或 \(R\)，让滤波器"自动调参"。
动机¶
回顾 §A1.5：Kalman 增益 \(K = P^-H^\top(HP^-H^\top+R)^{-1}\) 的分母同时包含 \(P^-\)（由 \(Q\) 决定）和 \(R\)。如果 \(Q\) 偏小，\(P^-\) 偏小，增益偏小，滤波器过度相信预测——过自信。如果 \(R\) 偏小，增益偏大，滤波器过度追踪噪声——估计抖动。工程师通常花大量时间"调 \(Q/R\)"，但这本质上是在人工完成一个可以自动化的任务。
历史：自适应 KF 的开创性工作来自 Mehra (1970, IEEE TAC 15(2):175–184)，提出基于新息序列估计噪声参数的三类方法（Bayesian、maximum-likelihood、correlation）。Sage & Husa (1969, Automatica 5(5):613–627) 提出了递推 \(Q\) 估计的滑动窗口公式。这两篇是后续所有自适应滤波工作的源头。
新息序列的统计性质¶
一致 KF 的新息 \(\tilde y_k = y_k - H_k\hat x_{k|k-1}\) 满足（§A1.17）：
- 零均值：\(\mathbb{E}[\tilde y_k] = 0\)
- 白噪声：\(\mathbb{E}[\tilde y_k\tilde y_j^\top] = S_k\delta_{kj}\)，其中 \(S_k = H_kP_{k|k-1}H_k^\top + R_k\)
关键观察：\(S_k\) 是新息的**理论协方差**，而 \(\tilde y_k\tilde y_k^\top\) 是**单次样本估计**。如果我们积累一段新息序列，其样本协方差应当接近 \(S_k\)——如果两者不一致，要么 \(Q\) 错了，要么 \(R\) 错了。
方法一：Innovation-Based R 估计（Mehra 1970）¶
从新息自协方差出发。取滑动窗口 \(N\)：
理论上 \(\hat C_0 \to S_k = HP_{k|k-1}H^\top + R\)，因此：
问题：\(\hat C_0\) 是有限样本估计，方差较大；\(N\) 太小则估计不稳定，\(N\) 太大则不能跟踪时变 \(R\)。经验值 \(N\in[10,50]\)。
如果 \(\hat R_k\) 的某个特征值变负会怎样？ 这说明样本新息协方差 \(\hat C_0\) 小于 \(HP^-H^\top\)，物理上不合理（"观测噪声为负"）。通常是因为 \(N\) 太小导致统计波动，或者 \(Q\) 本身就偏大。工程上必须对 \(\hat R_k\) 做 eigenvalue clipping：把负特征值截到一个正下界 \(\epsilon\)。
方法二：Sage-Husa 自适应 Q 估计¶
Sage & Husa (1969) 的递推估计公式：
其中 \(d_k = (1-b)/(1-b^{k+1})\)，\(b\in(0,1)\) 是遗忘因子（常取 \(b=0.95\sim 0.99\)）。
直觉：更新后的"剩余不确定性" \(P_{k|k}\) 加上被 Kalman 增益吸收的新息能量 \(K\tilde y\tilde y^\top K^\top\)，应当等于预测不确定性 \(FP_{k-1}F^\top + Q\)。差值就是 \(Q\) 的估计。
方法三：Fading Memory Filter¶
不直接估计 \(Q/R\)，而是给旧信息加**遗忘因子** \(\alpha_f > 1\)：
\(\alpha_f = 1\) 退化为标准 KF。\(\alpha_f > 1\) 人为膨胀协方差，防止滤波器对旧数据过度自信。等效地，这相当于把 \(Q\) 增大为 \((\alpha_f^2 - 1)FPF^\top + Q\)。
优点：实现极简（一行代码改动）、无需窗口缓存。
缺点：\(\alpha_f\) 对所有状态方向等比膨胀，无法区分"哪些方向需要放松、哪些不需要"。
Covariance Matching（协方差匹配）¶
统一视角：所有 innovation-based 自适应方法的核心是同一个等式：
左边可以从数据估计，右边依赖 \(Q\)（通过 \(P_{k|k-1}\)）和 \(R\)。同时估计 \(Q\) 和 \(R\) 一般是不可辨识的——必须固定其中一个或加约束。工程上最常见的做法是：\(R\) 从静止数据或传感器手册确定，只在线调 \(Q\)。
自适应 KF 的适用边界¶
| 场景 | 自适应 KF 是否有帮助 | 解释 |
|---|---|---|
| \(Q/R\) 缓慢时变（温漂、磨损） | 有帮助 | 参数缓变，遗忘因子可跟踪 |
| \(Q/R\) 初始猜测偏差大但恒定 | 有帮助 | 足够长的新息序列可收敛 |
| 模型结构错误（漏建状态） | 无法修复 | \(Q\) 被错误膨胀来补偿结构偏差，估计不一致 |
| 非平稳/突变系统 | 危险 | 旧数据的统计与当前不一致，自适应可能滞后或振荡 |
| 高维状态（\(n > 20\)） | 慎用 | \(Q\) 矩阵自由度 \(O(n^2)\)，样本效率极低 |
本质洞察：自适应 KF 调节的是模型参数（\(Q, R\)），而不是模型结构。如果你的运动模型漏掉了关键状态（如 IMU bias），自适应 KF 会通过膨胀 \(Q\) 来"掩盖"模型错误——短期内 NEES 看起来正常了，但长期估计精度反而下降，因为滤波器在用"增大不确定性"代替"修正模型"。正确做法是先修模型（加入 bias 状态、修正运动学），再考虑是否需要自适应。
⚠️ 思维陷阱：认为自适应 KF 是"自动调参"的万能工具。 自适应 KF 的理论前提是模型结构正确但参数未知。如果模型结构本身有问题（如用常速度模型跟踪机动目标、忘记建模 bias），自适应只会把 \(Q\) 调到不合理的大值来吸收误差——等价于"不信预测、只追量测"，失去了 KF 融合信息的核心价值。
⚠️ 编程陷阱：Sage-Husa 公式中忘记对 \(\hat Q_k\) 做对称化和正定保护。 递推估计中浮点误差会让 \(\hat Q_k\) 失去对称性甚至出现负特征值。每步更新后必须 (1) 对称化 \(\hat Q_k\leftarrow\frac{1}{2}(\hat Q_k+\hat Q_k^\top)\)；(2) 做 eigenvalue clipping 保证正半定。
练习¶
- 对标量系统 \(F=1, H=1, Q_{\text{true}}=0.5, R_{\text{true}}=1\)，设初始 \(\hat Q_0 = 2\)（偏大两倍），用 Sage-Husa 公式在 200 步后观察 \(\hat Q_k\) 是否收敛到 0.5。遗忘因子取 \(b=0.98\)。
- 解释为什么同时在线估计 \(Q\) 和 \(R\) 会导致不可辨识——提示：写出 \(S = HPH^\top + R\) 中 \(P\) 对 \(Q\) 的依赖链。
- 在 §A1.30 的 C++ KF 代码上增加 fading memory 功能：只需在预测步加一个
alpha_f系数。设计一个 GPS 跳变场景，观察 \(\alpha_f = 1.0\) 和 \(\alpha_f = 1.05\) 下滤波器恢复速度的差异。
§A1.9 信息形式的动机与定义 ⭐⭐¶
信息矩阵（precision matrix） \(\Lambda = P^{-1}\)、信息向量 \(\eta = \Lambda\hat x\)。高斯密度的两种等价参数化： $\(p(x)\propto\exp\!\Big(\!-\tfrac12(x-\hat x)^\top P^{-1}(x-\hat x)\Big)\propto\exp\!\Big(\!-\tfrac12 x^\top\Lambda x + \eta^\top x\Big)\)$
为什么需要信息形？ 三大动机： - 多传感器融合天然并行。\(N\) 个独立量测 \(\{(H_i, R_i, y_i)\}_{i=1}^N\) 的后验（在平坦先验下）为 \(\Lambda = \sum_i H_i^\top R_i^{-1}H_i,\ \eta = \sum_i H_i^\top R_i^{-1}y_i\)——纯加法，可任意顺序、可并行。协方差形必须串行 \(K_i\) 乘法。 - SLAM 后端稀疏。归一化信息矩阵 \(\bar\Lambda\) 只在"共享观测"的变量间有非零块，随路标数接近**带状稀疏**；对比协方差几乎稠密。SEIF/iSAM/g2o/GTSAM 都基于此观察。 - 无先验初始化合法。\(\Lambda_0 = 0\) 直接表示"完全无先验知识"；协方差形需 \(P_0 = \infty\) 数值不可行。
参考：Barfoot 2ed §3.3；Thrun, Burgard, Fox Probabilistic Robotics Ch. 3.4；Bar-Shalom §7.2。
§A1.10 Information Filter 预测与更新公式 ⭐⭐¶
更新步（信息加法形式，比协方差形更简单）： $\(\boxed{\,\Lambda_{k|k} = \Lambda_{k|k-1} + H_k^\top R_k^{-1}H_k\,}\)$ $\(\boxed{\,\eta_{k|k} = \eta_{k|k-1} + H_k^\top R_k^{-1}y_k\,}\)$ 仅需对 \(m\times m\) 的 \(R\) 求逆（若 \(R\) 对角，甚至无需求逆）。
预测步（比协方差形更复杂，需要两次求逆；当使用 Woodbury 加速形式时需 \(F\) 可逆，但下式本身只要 \(Q \succ 0\) 即可）： $\(\Lambda_{k|k-1} = (F_{k-1}\Lambda_{k-1|k-1}^{-1}F_{k-1}^\top + Q_{k-1})^{-1}\)$ $\(\eta_{k|k-1} = \Lambda_{k|k-1}F_{k-1}\Lambda_{k-1|k-1}^{-1}\eta_{k-1|k-1}\)$
用 Woodbury 可把两次求逆合并为一次 \((n+q)\times(n+q)\) 求逆（Thrun 2005 Table 3.6），但总体仍比协方差形预测贵。
KF 协方差形 vs 信息形对偶对照表¶
| 方面 | 协方差形 KF | 信息形 IF |
|---|---|---|
| 参数 | \(\hat x,\ P\) | \(\eta = P^{-1}\hat x,\ \Lambda = P^{-1}\) |
| 预测（时间传播） | 简单：\(P^- = FPF^\top + Q\) | 复杂：双重求逆 |
| 更新（量测融合） | 复杂：需 \(S^{-1}\)、增益 \(K\) | 简单：\(\Lambda^+ = \Lambda^- + H^\top R^{-1}H\) |
| 多传感器融合 | 实现上串行，数学上可交换（Joseph 稳定形式下顺序不影响最终结果；简化形式下数值舍入可引入微小顺序依赖） | 可并行求和 |
| 无先验初始化 | 不可行（\(P_0=\infty\)） | 自然（\(\Lambda_0=0\)） |
| 变量边缘化 | 简单（取行列子块） | 需 Schur 补 |
| 变量条件化 | 需 Schur 补 | 简单（取行列子块） |
| 典型场景 | 少量传感器、低维观测、频繁预测 | 多传感器融合、SLAM 后端、高维观测 |
工程启示。把预测频繁但量测稀疏的场景（IMU 积分 + 低频 GPS）放协方差形；把量测密集但时间步稀疏的场景（SLAM batch、多相机同步）放信息形。EKF-SLAM 和 SEIF 正对应此二极端。
如果不使用信息形，多传感器融合会怎样？ 假设有 8 个 UWB 基站同一时刻给出距离量测。协方差形必须串行处理：先用第 1 个量测算 \(K_1\)，更新 \(P\)，再用第 2 个量测算 \(K_2\)，更新 \(P\)，依次 8 轮。每轮都涉及矩阵乘法和 Joseph form 更新。信息形则是：\(\Delta\Lambda = \sum_{i=1}^8 H_i^\top R_i^{-1}H_i\)，\(\Delta\eta = \sum_{i=1}^8 H_i^\top R_i^{-1}y_i\)，一次加法完成，可以并行计算每个 \(\Delta\Lambda_i\) 后求和。对 SLAM 后端更极端：1000 个路标观测的信息形更新只是 1000 次稀疏矩阵加法，而协方差形需要 1000 次稠密矩阵更新。这就是 GTSAM/g2o 选择信息矩阵（Hessian）作为核心数据结构的根本原因。
§A1.11 Extended Information Filter (EIF) 与 SEIF ⭐⭐⭐¶
EIF 就是 EKF 的信息形式：在当前估计 \(\hat x\) 处线性化 \(h\) 得 Jacobian \(H_k\)，更新变为 $\(\Lambda_{k|k} = \Lambda_{k|k-1} + H_k^\top R_k^{-1}H_k\)$ $\(\eta_{k|k} = \eta_{k|k-1} + H_k^\top R_k^{-1}\big[y_k - h(\hat x_{k|k-1}) + H_k\hat x_{k|k-1}\big]\)$ 代价：必须周期性恢复均值 \(\hat x = \Lambda^{-1}\eta\) 用于下次 Jacobian 计算。
SEIF（Sparse Extended Information Filter）——Thrun, Liu, Koller, Ng, Ghahramani, Durrant-Whyte (2004) IJRR 23(7-8):693–716。核心观察：SLAM 归一化信息矩阵经验上接近稀疏，可主动稀疏化而近似误差可控。
SEIF 三个关键算子： 1. 更新：量测只涉及少量变量 ⇒ \(H^\top R^{-1}H\) 只在相关块贡献非零，稀疏结构保持。 2. 运动更新：运动把 \(x_t\) 与所有 active 路标耦合 → 信息矩阵变 dense ⇒ 需要稀疏化。 3. 稀疏化 (sparsification)：把路标集划 \(M = m_0\cup m_+\cup m_-\)，用条件独立近似 \(p(x_t, m_+, m_0, m_-)\approx p(x_t\mid m_+, m_0)p(m_0, m_+, m_-)\)，Schur 补把 \(x_t\)–\(m_0\) 链接置零，常数时间完成。 4. 均值恢复：Gauss–Seidel relaxation 利用 \(\Lambda\) 稀疏对角占优，amortized \(O(1)\)。
复杂度对比（\(N\) = 路标数）：
| 操作 | EKF-SLAM | SEIF |
|---|---|---|
| 运动更新 | \(O(N^2)\) | \(O(1)\) |
| 观测更新 | \(O(N^2)\) | \(O(1)\) |
| 存储 | \(O(N^2)\) | \(O(N)\) |
警告：Eustice–Walter–Leonard (2007) 证实 SEIF 稀疏化在全局坐标下**过度自信 (overconfident)**，提出 ESEIF (Exactly Sparse EIF) 修正。SEIF 是从 EKF-SLAM 到现代因子图 SLAM 的概念桥梁。
信息矩阵 = 因子图 Hessian（与 5-B 连接）。非线性最小二乘 \(\hat x = \arg\min\sum_i\|r_i(x)\|^2_{\Sigma_i^{-1}}\) 的 Gauss–Newton 正规方程系数为 $\(H = \sum_i J_i^\top\Sigma_i^{-1}J_i = J^\top\Sigma^{-1}J = \Lambda\)$ 这正是后验信息矩阵。每个因子只触及其连接的变量 ⇒ \(J_i\) 稀疏 ⇒ \(\Lambda\) 块稀疏。因子图结构 ↔ \(\Lambda\) 的稀疏模式 ↔ AMD/COLAMD 排序 ↔ 稀疏 Cholesky \(\Lambda = R^\top R\)——这就是 GTSAM/g2o/iSAM2 的代数核心。
§A1.12 数值稳定性问题——为什么需要 Square Root Filter ⭐⭐¶
三大经典故障模式：
- \(P = (I - KH)P\) 丢失正定/对称性。简化形仅对最优 \(K\) 精确成立，减法消相抵 (catastrophic cancellation) 会让 \(P\) 失对称、失 PSD，下一次 Cholesky 崩溃。
- 长期 trace(P) 漂移。反复乘加累积舍入 → \(P\) 要么过大（发散 divergence）要么过小（过度自信、收敛到错解）。经典论述：R. Fitzgerald, "Divergence of the Kalman Filter," IEEE TAC 16(6):736–747, 1971。
- 大条件数 (ill-conditioning)。精确量测 \(R\) 小、弱可观分量、大尺度状态（km 位置 + m/s 速度）并存时，\(\mathrm{cond}(P)\) 可达 \(10^{10}\) 以上。精度损失比：每次协方差更新损失约 \(\log_{10}\mathrm{cond}(P)\) 位有效数。
平方根形的关键数值定理： $\(\boxed{\,\mathrm{cond}(S) = \sqrt{\mathrm{cond}(P)}\,}\)$ 存 \(S\) 使 \(P = SS^\top\)，有效位**翻倍**。这一句话解释了平方根滤波器为何成为航天/GNSS 工业标准。
Apollo 历史背景。James E. Potter (MIT Instrumentation Lab, 1963) 为 Apollo 导航计算机 (AGC) 推导首个 square root filter：AGC 仅 15-bit 定点、4KB 指令存储，原始 KF 在登月导航仿真中协方差爆炸；Potter 一个周末内推出 \(P = SS^\top\) 的 rank-1 更新，次周部署，Apollo 11 登月成功。论文：Potter & Stern, "Statistical Filtering of Space Navigation Measurements," AIAA Guidance, Control, and Flight Mechanics Conference, 1963 (Paper 63-333)；MIT IL Space Guidance Analysis Memo #40 (Potter 1963) 内部版。
⚠️ 概念误区：认为平方根滤波器只在低精度硬件上才需要。 即使在 64-bit 浮点 CPU 上，当状态维度较高（\(n\ge 15\)）且系统长时间运行（数小时连续 VIO），条件数 \(\mathrm{cond}(P)\) 可以轻松超过 \(10^{10}\)，此时每步更新损失 10 位有效精度。平方根形把条件数降到 \(10^5\)，损失仅 5 位。这不是理论上的差异——这是"跑 30 分钟后 Cholesky 突然崩溃"和"稳定跑 8 小时"的差异。
练习¶
- 对 \(P=\mathrm{diag}(10^{-3},\,10^3)\)，计算 \(\mathrm{cond}(P)\) 和 \(\mathrm{cond}(S)\)（\(S=\text{chol}(P)\)）。双精度浮点约 15 位有效数字，每种形式分别能保留多少位？
- 解释为什么"条件数平方根"定理 \(\mathrm{cond}(S)=\sqrt{\mathrm{cond}(P)}\) 直接来自 \(P=SS^\top\) 和奇异值关系 \(\sigma_i(P)=\sigma_i(S)^2\)。
§A1.13 Potter's Square Root Filter（Cholesky 形式） ⭐⭐⭐¶
核心：存 \(S\) 满足 \(P = SS^\top\)（一般平方根，不必三角）。
标量量测更新（\(y = h^\top x + v,\ v\sim\mathcal{N}(0,r)\)）： $\(f = S^\top h,\quad \alpha = 1/(f^\top f + r),\quad \gamma = 1/(1+\sqrt{\alpha r})\)$ $\(K = \alpha\,S\,f,\quad S^+ = S - \gamma K f^\top,\quad \hat x^+ = \hat x + K(y - h^\top\hat x)\)$ \(O(n^2)\) 每标量量测 + 1 次开方。正定性 + 对称性免费保证（\(SS^\top\) 永远 PSD）。
**预测**需含过程噪声 \(Q\)：构造 \([FS,\ \sqrt{Q}]\)（\(n\times(n+q)\)），Householder/Givens QR 三角化得新 \(\tilde S\)，使 \(\tilde S\tilde S^\top = FSS^\top F^\top + Q\)。
注意：Potter 1963 原文只处理 \(Q = 0\)（Apollo 无过程噪声建模），QR-预测是 Andrews (1968)、Kaminski–Bryson–Schmidt (1971) 补上的。
§A1.14 UD 分解滤波器（Bierman / Thornton） ⭐⭐⭐¶
分解：\(P = UDU^\top\)，其中 \(U\) 单位上三角（对角为 1），\(D\) 对角（\(D_i \ge 0\)）。与 Cholesky 关系：若 \(P = SS^\top\)，\(S\) 上三角，则 \(S = U\sqrt D\)。"Square-root-free Cholesky"。
为什么避免开方？ - 1970 年代小型机开方比乘除贵数倍；现代 CPU 仍 5–10× 除法成本。 - 无开方 ⇒ 可用整数/定点算术，适合 GPS 接收机 ASIC、MEMS IMU、航天飞行软件。 - 实测 (Bierman-Thornton)：UD 计算量比 Potter 省 30–50%，比标准 Kalman 高 ~20%，数值稳定性接近 Potter。
Bierman (1976) 标量量测更新伪代码¶
引用：G. J. Bierman, "Measurement Updating Using the U-D Factorization," Automatica 12(4):375–382, 1976。
# 输入：U 是 n×n 单位上三角矩阵，D 是对角向量，h 是量测向量，r 是标量噪声方差
# 输出：更新后 U, D, 增益 K
f = U.T @ h # 回代得 f
g = D * f # 分量乘
alpha = r + f[0]*g[0]
D[0] = D[0] * r / alpha
K = g.copy() # 累积增益
for j in 1..n-1:
beta = alpha
alpha = alpha + f[j]*g[j]
lam = -f[j] / beta
D[j] = D[j] * beta / alpha
for i in 0..j-1:
tau = U[i,j]
U[i,j] = tau + lam * K[i]
K[i] = K[i] + g[j]*tau
K[j] = g[j]
gain = K / alpha
x_hat = x_hat + gain * (y - h.T @ x_hat)
Bierman 非负保持定理：所有 \(D_j^+ \ge 0\) 无条件**成立（每步只乘正比例因子 \(\beta/\alpha\)）——**这是 UD 滤波器永不失 PSD 的根本保证。
向量量测处理：若 \(R\) 非对角，先对 \(R\) 做 UD 分解 \(R = U_RD_RU_R^\top\)，变换 \(\tilde y = U_R^{-1}y\)、\(\tilde H = U_R^{-1}H\) 使噪声去相关，再逐标量处理。
Thornton (1977) 时间更新（MWGS 算法）¶
引用：Thornton & Bierman, "Gram–Schmidt Algorithms for Covariance Propagation," Int. J. Control 25(2):243–260, 1977。
目标：由 \((U^+, D^+)\)、\(F\)、\((G, Q = \mathrm{diag})\) 得 \((\bar U, \bar D)\) 使 \(\bar U\bar D\bar U^\top = F(U^+D^+U^{+\top})F^\top + GQG^\top\)。
W = [F @ U_plus, G] # 拼接状态传播项和过程噪声项
Dtilde = concat([D_plus, Q_diag]) # 拼接原协方差对角和过程噪声对角
for i = n-1 downto 0:
D_bar[i] = <W[i], W[i]>_{Dtilde} # 加权内积
for j = 0..i-1:
U_bar[j,i] = <W[j], W[i]>_{Dtilde} / D_bar[i]
W[j] = W[j] - U_bar[j,i] * W[i]
Square Root Filter 变体对比表¶
| 变体 | 年份 | 分解 | 量测更新 | 时间更新 | 复杂度 | 开方 | 典型应用 |
|---|---|---|---|---|---|---|---|
| Potter–Stern | 1963 | \(P=SS^\top\)（一般） | rank-1 mod + 1 sqrt | 无 Q 原版 / QR | \(O(n^2)\) 更新 | 有 | Apollo AGC |
| Andrews | 1968 | \(P=SS^\top\) | Householder | Householder QR | \(O(n^3)\) | 有 | 理论统一 |
| Kaminski–Bryson–Schmidt | 1971 | \(P=SS^\top\) | Householder/Givens | QR | \(O(n^3)\) | 有 | Cov/Info 对偶综述 |
| Carlson | 1973 | \(P=SS^\top\)，\(S\) 上三角 | 解析 rank-1 保三角 | 保三角 | ~50% 快过 Potter | 有 | 嵌入式、航天 |
| Bierman UD | 1976 | \(P=UDU^\top\) | 无开方 rank-1 | — | 与 std KF 同阶 | 无 | GPS/INS 主流 |
| Thornton UD | 1977 | \(P=UDU^\top\) | — | MWGS 加权正交化 | \<20% 额外开销 | 无 | 与 Bierman 配对 |
| SRIF（Dyer–McReynolds / Bierman 书） | 1969/77 | 信息形 \(\Lambda=R^\top R\) | Householder 合并数据阵 | Dyer–McReynolds QR | \(O(n^3)\) | 有 | JPL ODP |
| SR-UKF (van der Merwe–Wan) | 2001 | \(P=SS^\top\) + sigma points | Cholesky up/downdate + QR | QR | \(O(n^3)\) | 有 | 现代 UKF |
工程选择： - C++ 嵌入式 / 航天飞控：Bierman+Thornton UD（Grewal–Andrews 书 Ch. 6 附完整 MATLAB 实现）。 - VIO / SLAM 前端：通常 Joseph form 足够（float64 精度 + 状态维度 15–30）；若出现长时间数值漂移可换 UD。 - 深空轨道确定 (JPL ODP, Cassini/Juno/Voyager)：SRIF。 - 现代 UKF：SR-UKF（van der Merwe-Wan 2001，见 5-A2）。
工程案例：NASA Orion GN&C 采用 UDU Kalman 为 backbone（Zanetti et al. "Information Formulation of the UDU Kalman Filter," PMC6443377）；Trimble/u-blox/Septentrio GNSS 接收机 float-ambiguity 滤波普遍 Bierman–Thornton UD。
§A1.15 NEES（Normalized Estimation Error Squared） ⭐⭐¶
定义： $\(\varepsilon_k = (x_k - \hat x_k)^\top P_k^{-1}(x_k - \hat x_k)\)$ 若估计器**一致**（consistent，即 \(P_k\) 正确反映真实误差协方差），则 \(\varepsilon_k \sim \chi^2_n\)，\(n\) 为状态维度，\(\mathbb{E}[\varepsilon_k] = n\)。
Monte Carlo 平均 NEES（Bar-Shalom 约定，§5.4.2 式 5.4.2-4）： $\(\bar\varepsilon_k = \frac{1}{N}\sum_{i=1}^N\varepsilon_k^{(i)},\qquad N\bar\varepsilon_k\sim\chi^2_{Nn}\)$ 95% 置信区间： $\(\bar\varepsilon_k\in\left[\frac{\chi^2_{Nn,\alpha/2}}{N},\ \frac{\chi^2_{Nn,1-\alpha/2}}{N}\right]\)$ （期望 = \(n\)；注意分母为 \(N\)，非 \(Nn\)。）
典型数字（已用 scipy.stats.chi2.ppf 核算）：
- \(n=3, N=50, \alpha=0.05\) → \([2.360,\ 3.716]\)
- \(n=6, N=100, \alpha=0.05\) → \([5.340,\ 6.698]\)
- \(n=15, N=100, \alpha=0.05\) → \([13.946,\ 16.092]\)（VIO 15 维状态）
判读： - \(\bar\varepsilon_k\) 超上界 ⇒ 过自信 (overconfident)：\(P_k\) 太小，真实误差更大，常因 \(Q\) 设小、忽略线性化误差、FEJ 未应用（见 5-A2）。 - \(\bar\varepsilon_k\) 低于下界 ⇒ 过保守 (conservative)：\(P_k\) 太大，可能 \(R\) 设大或重复融合同一信息。 - 区间内 ⇒ 统计一致。
限制：NEES 需**真值**（\(x_k\)），仅仿真或带 MoCap 的数据集可用；实车在线用 NIS。
§A1.16 NIS（Normalized Innovation Squared） ⭐⭐¶
定义： $\(\epsilon_k = \tilde y_k^\top S_k^{-1}\tilde y_k,\qquad \tilde y_k = y_k - H_k\hat x_{k|k-1},\ S_k = H_kP_{k|k-1}H_k^\top + R_k\)$ 若一致，\(\epsilon_k\sim\chi^2_m\)，\(m\) 为观测维度，\(\mathbb{E}[\epsilon_k] = m\)。
工程价值：NIS 不需要真值，完全由滤波器内部量（\(\tilde y, S\)）构成，可**在线**逐步计算。因此：
- 工程诊断的**首选**指标（Bar-Shalom §5.4.2）。
- MSCKF/OpenVINS/VINS-Fusion 的 \(\chi^2\) gate 实际用 NIS（见 §A1.18）。
- FilterPy KalmanFilter.mahalanobis 属性即 \(\sqrt{\epsilon_k}\)。
Monte Carlo 平均 NIS：与 NEES 完全平行，\(\bar\epsilon_k\in[\chi^2_{Nm,\alpha/2}/N,\ \chi^2_{Nm,1-\alpha/2}/N]\)。
§A1.17 Innovation Whiteness Test ⭐⭐¶
原理：一致 KF 的新息序列 \(\{\tilde y_k\}\) 应为零均值白噪声。若存在时间相关，说明模型有偏差（未建模的动力学、时变 bias、错误 Jacobian 等）。
样本自相关（Bar-Shalom §5.4.3 式 5.4.3-2）： $\(\hat\rho_\tau = \frac{\sum_{k=1}^{N-\tau}\tilde y_k^\top\tilde y_{k+\tau}}{\sqrt{\sum\tilde y_k^\top\tilde y_k\cdot\sum\tilde y_{k+\tau}^\top\tilde y_{k+\tau}}}\)$
95% 白噪置信带：\(\pm 1.96/\sqrt{N}\)。若大部分 \(\tau>0\) 的 \(\hat\rho_\tau\) 落入带内即可判为白。
诊断意义：自相关持续正 ⇒ 模型欠拟合（miss 动态）；频繁越带 ⇒ \(Q\) 设小导致过度平滑。
§A1.18 \(\chi^2\) Gate 用于异常观测剔除 ⭐⭐¶
规则：若 \(\epsilon_k = \tilde y_k^\top S_k^{-1}\tilde y_k > \chi^2_{m, 1-\alpha}\) 则**拒绝该量测**。
常用阈值（\(\alpha = 0.05\)）： - \(m=1\)：3.841 - \(m=2\)（视觉像素观测）：5.991 ✅ - \(m=3\)（3D 位置观测）：7.815
\(\chi^2\) 临界值速查表（\(\chi^2_{n, 1-\alpha}\)）¶
| df \(n\) | \(\alpha=0.10\) | \(\alpha=0.05\) | \(\alpha=0.01\) |
|---|---|---|---|
| 1 | 2.706 | 3.841 | 6.635 |
| 2 | 4.605 | 5.991 | 9.210 |
| 3 | 6.251 | 7.815 | 11.345 |
| 4 | 7.779 | 9.488 | 13.277 |
| 5 | 9.236 | 11.070 | 15.086 |
| 6 | 10.645 | 12.592 | 16.812 |
| 7 | 12.017 | 14.067 | 18.475 |
| 8 | 13.362 | 15.507 | 20.090 |
| 9 | 14.684 | 16.919 | 21.666 |
| 10 | 15.987 | 18.307 | 23.209 |
| 11 | 17.275 | 19.675 | 24.725 |
| 12 | 18.549 | 21.026 | 26.217 |
| 13 | 19.812 | 22.362 | 27.688 |
| 14 | 21.064 | 23.685 | 29.141 |
| 15 | 22.307 | 24.996 | 30.578 |
OpenVINS 实战代码（ov_msckf/src/update/UpdaterMSCKF.cpp, UpdaterSLAM.cpp, UpdaterZeroVelocity.cpp）：
#include <boost/math/distributions/chi_squared.hpp>
Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order);
Eigen::MatrixXd S = H * P_marg * H.transpose() + R;
double chi2 = res.dot(S.llt().solve(res)); // 归一化新息平方
double chi2_check;
if (res.rows() < 1000) {
chi2_check = chi_squared_table[res.rows()]; // 预计算查表
} else {
boost::math::chi_squared dist(res.rows());
chi2_check = boost::math::quantile(dist, 0.95);
}
// 注意源码字段名写作 chi2_multipler（不是 multiplier）
if (chi2 > _options.chi2_multipler * chi2_check) {
continue; // 剔除该特征
}
核对要点：OpenVINS 实际用的是 NIS（无真值），这印证 NIS 在工程界的中心地位；chi2_multipler 是用户可调的放松乘子（典型 1.0–3.0），MSCKF 中可针对不同量测类型独立调。
关联：\(\chi^2\) gate 是硬阈值；更平滑的替代是 M-estimator（Huber、Cauchy 等），把二次损失换成 sub-quadratic 损失——详见 5-F 鲁棒估计章。
一致性诊断工具对比表¶
| 工具 | 公式 | 分布 | 需要真值 | 在线可用 | 主要诊断 |
|---|---|---|---|---|---|
| NEES | \((x-\hat x)^\top P^{-1}(x-\hat x)\) | \(\chi^2_n\) | 是 | 仿真/MoCap | 过自信 vs 过保守 |
| NIS | \(\tilde y^\top S^{-1}\tilde y\) | \(\chi^2_m\) | 否 | 是 | 同上，工程首选 |
| Innovation whiteness | 自相关 \(\hat\rho_\tau\) | \(\pm 1.96/\sqrt N\) 带 | 否 | 是 | 模型欠拟合 / 未建模动态 |
| \(\chi^2\) gate | \(\epsilon_k > \chi^2_{m,1-\alpha}\) | 阈值 | 否 | 是 | 单帧异常观测剔除 |
§A1.19 C++/Python 库映射 ⭐⭐¶
库与 API 对照表¶
| 库 | 类 / 函数 | 头文件 / 模块 | 典型场景 |
|---|---|---|---|
| FilterPy | KalmanFilter, InformationFilter, SquareRootKalmanFilter, FixedLagSmoother, ExtendedKalmanFilter, UnscentedKalmanFilter |
filterpy.kalman |
Python 原型、教学、NEES 仿真 |
| FilterPy stats | NEES(xs, est_xs, Ps), mahalanobis(x, μ, Σ) |
filterpy.stats |
一致性诊断 |
| Eigen | .llt(), .ldlt(), .solve(), .selfadjointView<Lower>() |
<Eigen/Dense>, <Eigen/Cholesky> |
C++ 手写 KF 底层算子 |
| GTSAM | ExtendedKalmanFilter<VALUE>（线性退化）+ gtsam::KalmanFilter 函数式版 |
gtsam/nonlinear/ExtendedKalmanFilter.h |
SRIF 后端；桥到因子图 |
| robot_localization | RobotLocalization::FilterBase、Ekf、Ukf、RosFilter<T>、checkMahalanobisThreshold() |
include/robot_localization/ |
ROS 融合 odom/IMU/GPS |
| MATLAB Control SysTbx | kalman(sys,Q,R,N)、extendedKalmanFilter、Simulink Kalman Filter block |
— | 稳态 KF、时变 EKF |
| MATLAB Sensor Fusion Tbx | trackingKF / trackingEKF / trackingUKF |
— | 多目标跟踪 |
| MATLAB CV Tbx | vision.KalmanFilter, configureKalmanFilter |
— | 视觉目标跟踪 |
| kalmanif (artivis) | ExtendedKalmanFilter、InvariantEKF、UnscentedKalmanFilterManifolds（无纯线性 KF 基类；基于 manif 流形） |
header-only C++17 | Lie 群 EKF/IEKF/UKF-M |
| OpenVINS | UpdaterMSCKF::chi2_check、UpdaterSLAM 中的 \(\chi^2\) gate |
ov_msckf/src/update/ |
VIO 异常剔除参考实现 |
FilterPy 最小工作示例（2D CV 目标跟踪）：
import numpy as np
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
kf = KalmanFilter(dim_x=4, dim_z=2) # 状态 [px,py,vx,vy]
dt = 0.1
kf.F = np.array([[1,0,dt,0],[0,1,0,dt],[0,0,1,0],[0,0,0,1]])
kf.H = np.array([[1,0,0,0],[0,1,0,0]])
kf.R = np.eye(2) * 0.5**2 # 量测噪声
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.1, block_size=2)
kf.x = np.zeros(4); kf.P *= 10.
for z in zs:
kf.predict(); kf.update(z)
print('NIS', kf.mahalanobis**2, 'gain', kf.K.diagonal())
Eigen C++ 手写 KF 骨架（Joseph form）：
#include <Eigen/Dense>
using Eigen::MatrixXd; using Eigen::VectorXd;
// 预测：用线性动力学传播均值与协方差
VectorXd xp = F * x + B * u;
MatrixXd Pp = F * P * F.transpose() + Q;
// 更新：用 Joseph form 保持协方差数值稳定
VectorXd y = z - H * xp;
MatrixXd S = H * Pp * H.transpose() + R;
MatrixXd K = (S.llt().solve(H * Pp)).transpose(); // 数值稳定
x = xp + K * y;
MatrixXd IKH = MatrixXd::Identity(n,n) - K * H;
P = IKH * Pp * IKH.transpose() + K * R * K.transpose();
double nis = y.transpose() * S.llt().solve(y);
§A1.20 学习资源¶
免费教材 PDF： - Barfoot, State Estimation for Robotics 2ed (2024)：https://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf - Särkkä & Svensson, Bayesian Filtering and Smoothing 2ed (2023)：https://users.aalto.fi/~ssarkka/pub/bfs_book_2023_online.pdf - Thrun, Burgard, Fox, Probabilistic Robotics (2005)：MIT Press（非免费，但多数章节在作者主页）
视频： - Cyrill Stachniss (Uni Bonn)：Photogrammetry II 播放列表（Lecture 14 KF & EKF）、SLAM Course 2013/14 (Lec 3-4 KF/EKF)、5 分钟速览版。YouTube 搜 "Stachniss Kalman"。 - Michel van Biezen："Special Topics 1 – The Kalman Filter" 播放列表，实际 55 讲（非 45 讲），从单维直到 Tracking-Airplane 多维例子。 - Roger Labbe Kalman-and-Bayesian-Filters-in-Python：https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python，nbviewer 在线读，配 FilterPy 实操。
中文： - 高翔《视觉 SLAM 十四讲》第二版：第 10 讲后端优化（EKF-SLAM vs BA 对比）；slambook2 GitHub 有完整代码。 - 深蓝学院课程：《状态估计与多传感器融合》、《多传感器融合定位》、贺一家《从零开始手写 VIO》（含 MSCKF + \(\chi^2\) gating 实现）。
§A1.21 学习时间预算¶
| 档位 | 内容 | 预算 |
|---|---|---|
| 档位 3（博士入学） | §A1.1–A1.8 完整读通，§A1.9–A1.11 IF 基本公式，§A1.12–A1.13 理解 Potter，§A1.15–A1.18 全部 + 一次仿真作 NEES | 20–25 h |
| 档位 4（进阶） | §A1.14 Bierman UD 完整伪代码实现 + 单元测试；§A1.8 DARE 手解一例；OpenVINS chi2 源码精读；SEIF 原论文精读 | +5–8 h |
§A1.22 自测题（共 10 道）¶
- 贝叶斯推导 KF。从 \(p(x_{k-1}\mid y_{1:k-1}) = \mathcal{N}(\hat x, P)\) 出发，用 Chapman–Kolmogorov + 配方法推出预测与更新的所有公式。
- 证明 KF 是 BLUE。对 \(\hat x_{k|k} = \hat x_{k|k-1} + K\tilde y_k\) 求 \(\partial\mathrm{tr}(P_{k|k})/\partial K = 0\)，并论证为何结果不依赖高斯假设。
- Information Filter 更新公式。从 \(p(x\mid y)\propto p(y\mid x)p(x)\) 出发，展开两个高斯的乘积，读出 \(\Lambda^+ = \Lambda^- + H^\top R^{-1}H\)。
- Woodbury 转换。给定 \((P^-, H, R)\)，用 Woodbury 证明 \((P^{-1}+H^\top R^{-1}H)^{-1}H^\top R^{-1} = P^-H^\top(HP^-H^\top+R)^{-1}\)。
- DARE 与 LQR 对偶。写出 LQR 的 DARE，指出与 KF DARE 之间"\(F\leftrightarrow F^\top,\ H\leftrightarrow B,\ Q\leftrightarrow Q_c,\ R\leftrightarrow R_c\)"对应；解释 Separation Principle 成立的必要条件。
- NEES \(\chi^2\) 检验。给定 \(n=6, N=100, \alpha=0.05\)，计算 \(\bar\varepsilon\) 置信区间（答案：\([5.340,\ 6.698]\)）。
- NIS 在线异常检测。写出接收 \(\tilde y_k, S_k\) 后判定"拒绝/接受"该量测的伪代码，阈值取 \(\chi^2_{m,0.95}\)。
- FilterPy 2D 跟踪 + NEES。用 FilterPy 实现匀速 2D 跟踪（状态 [px,py,vx,vy]），跑 \(N=50\) 次 Monte Carlo，画出 \(\bar\varepsilon_k\) 时序与 \([2.36,\ 3.72]\)（\(n=3\) 假设）或对应 \(n=4\) 区间。
- Bierman UD 实现。按 §A1.14 伪代码用 Python/Eigen 实现 UD 标量量测更新；验证对随机 SPD 矩阵与标量 \(h, r\) 的结果与常规 KF 一致。
- Innovation whiteness。对已跑完的 \(\tilde y_{1:N}\) 序列计算 \(\hat\rho_\tau,\ \tau=1..20\)，画 95% 置信带；若 \(\tau=1\) 明显越带说明什么？如何修正？
§A1.23 常见陷阱清单¶
| # | 陷阱 | 后果 | 修复 |
|---|---|---|---|
| 1 | 简化形 \(P=(I-KH)P\) 在次优 \(K\)/float32 下使用 | \(P\) 失对称、失 PSD，Cholesky 崩 | 始终用 Joseph form |
| 2 | \(P\) 每步不对称 | 数值漂移 | 每步强制 \(P\leftarrow(P+P^\top)/2\) |
| 3 | 初始 \(P_0\) 太大（\(10^{12}\)） | 条件数爆炸，首次 \(K\) 过大 | \(P_0\) 取各维度"合理量级平方"；或信息形 \(\Lambda_0 = 0\) |
| 4 | 初始 \(P_0\) 太小 | 早期估计锁死，不收敛 | 按物理量级设，IMU bias 初始 \(\sigma\) ~ 0.1 rad/s / m/s² |
| 5 | \(Q\) 设过小 | 过自信，滤波器发散 | NEES 监测 + tuning；常用技巧：\(Q\) 设为运动模型残差方差的 2–4× |
| 6 | \(R\) 设错（错误单位：rad vs deg） | 量测权重错，增益偏 | 写单元测试验 \(R\) 的物理单位 |
| 7 | \(\chi^2\) gate 阈值按 \(\alpha=0.05\) 太紧 | 好特征被误剔，退化场景更糟 | OpenVINS chi2_multipler（1.5–3.0）放松 |
| 8 | 信息形预测使用 Woodbury 加速形式时忘记 \(F\) 可逆要求 | NaN | 改用直接双重求逆公式（只需 \(Q\succ 0\)）；或确保动力学可逆 |
| 9 | UD 更新对向量 \(R\) 未做去相关直接逐标量 | 融合错，等效多次加同一信息 | 先 \(R = U_RD_RU_R^\top\) 去相关再标量处理 |
| 10 | NEES 只跑一次就下结论 | \(\chi^2_n\) 方差大，误判率高 | 至少 \(N\ge 30\) 次 Monte Carlo 平均 |
| 11 | NEES 混淆两种约定（期望 = \(n\) vs = 1） | 置信区间算错 | 本文 Bar-Shalom 约定：\(\bar\varepsilon = (1/N)\sum\varepsilon^{(i)}\)，期望 = \(n\)，区间分母 \(N\) |
| 12 | 忘记加过程噪声 \(Q\) | \(P\) 单调收缩到 0，滤波器冻结 | 每步 \(P^- = FPF^\top + Q\)，\(Q\) 不可省 |
| 13 | \(S\) 条件数差直接 S.inverse() |
Kalman gain 数值溢出 | S.llt().solve(...) 或 QR；SPD 保证用 Joseph 或 UD |
| 14 | 长时间不重置 Jacobian（EIF） | 信息形 \(\hat x = \Lambda^{-1}\eta\) 与真实线性化点偏离 | 周期性恢复均值并重新线性化 |
| 15 | 误把 FilterPy mahalanobis 当 NIS |
单位错（它是 \(\sqrt{\text{NIS}}\)） | NIS = kf.mahalanobis**2 |
关键公式速查表¶
| 名称 | 公式 |
|---|---|
| Chapman–Kolmogorov | \(p(x_k\mid y_{1:k-1}) = \int p(x_k\mid x_{k-1})p(x_{k-1}\mid y_{1:k-1})dx_{k-1}\) |
| Bayes 更新 | \(p(x_k\mid y_{1:k}) \propto p(y_k\mid x_k)p(x_k\mid y_{1:k-1})\) |
| KF 预测 | \(\hat x^- = F\hat x + Bu,\quad P^- = FPF^\top + Q\) |
| KF 更新 | \(K = P^-H^\top S^{-1},\ \hat x^+ = \hat x^- + K\tilde y,\ P^+ = (I-KH)P^-\) |
| Joseph form | \(P^+ = (I-KH)P^-(I-KH)^\top + KRK^\top\) |
| Woodbury | \((A+BCD)^{-1} = A^{-1}-A^{-1}B(C^{-1}+DA^{-1}B)^{-1}DA^{-1}\) |
| Information 更新 | \(\Lambda^+ = \Lambda^-+H^\top R^{-1}H,\ \eta^+ = \eta^-+H^\top R^{-1}y\) |
| DARE | \(P = FPF^\top+Q-FPH^\top(HPH^\top+R)^{-1}HPF^\top\) |
| NEES | \(\varepsilon = (x-\hat x)^\top P^{-1}(x-\hat x)\sim\chi^2_n\) |
| NIS | \(\epsilon = \tilde y^\top S^{-1}\tilde y\sim\chi^2_m\) |
| NEES CI | \(\bar\varepsilon\in[\chi^2_{Nn,\alpha/2}/N,\ \chi^2_{Nn,1-\alpha/2}/N]\) |
| Cond 翻倍 | \(\mathrm{cond}(S) = \sqrt{\mathrm{cond}(P)}\) for \(P = SS^\top\) |
核心教材/论文表¶
| 类型 | 引用 | 章节 |
|---|---|---|
| 教材 | Särkkä & Svensson, Bayesian Filtering and Smoothing 2ed (2023) | Ch. 3–6 |
| 教材 | Barfoot, State Estimation for Robotics 2ed (2024) | §3.1–3.3 |
| 教材 | Simon, Optimal State Estimation (2006) | Ch. 5–7 |
| 教材 | Thrun, Burgard, Fox, Probabilistic Robotics (2005) | §2, §3.4, Ch. 11 |
| 教材 | Bar-Shalom, Li, Kirubarajan, Estimation with Applications to Tracking and Navigation (2001) | §5.4, §7.2 |
| 教材 | Anderson & Moore, Optimal Filtering (1979) | Ch. 3–4 |
| 教材 | Grewal & Andrews, Kalman Filtering: Theory and Practice w/ MATLAB 4ed (2015) | Ch. 6（SR/UD 实现最详） |
| 教材 | Bierman, Factorization Methods for Discrete Sequential Estimation (1977, Dover reprint) | 全书 |
| 教材 | Maybeck, Stochastic Models, Estimation, and Control Vol. 1 (1979) | Ch. 7–8 |
| 论文 | Kalman, "A New Approach to Linear Filtering and Prediction Problems," ASME J. Basic Eng. 82(1):35–45, 1960 | 奠基 |
| 论文 | Rauch, Tung, Striebel, "Maximum Likelihood Estimates of Linear Dynamic Systems," AIAA J. 3(8):1445, 1965 | RTS smoother |
| 论文 | Potter & Stern, "Statistical Filtering of Space Navigation Measurements," AIAA Guidance & Control Conf., 1963 | SR-KF 起源 |
| 论文 | Kaminski, Bryson, Schmidt, "Discrete Square Root Filtering: A Survey," IEEE TAC AC-16(6):727–736, 1971 | Cov/Info SR 统一 |
| 论文 | Carlson, "Fast Triangular Formulation of the Square Root Filter," AIAA J. 11(9):1259–1265, 1973 | 三角化 SR |
| 论文 | Bierman, "Measurement Updating Using the U-D Factorization," Automatica 12(4):375–382, 1976 | UD 量测更新 |
| 论文 | Thornton & Bierman, "Gram-Schmidt Algorithms for Covariance Propagation," Int. J. Control 25(2):243–260, 1977 | UD 时间更新 |
| 论文 | Thrun et al., "Simultaneous Localization and Mapping with Sparse Extended Information Filters," IJRR 23(7-8):693–716, 2004 | SEIF |
| 论文 | Eustice, Walter, Leonard, "Exactly Sparse Extended Information Filters," IJRR 2007 | ESEIF 改进 |
| 论文 | Fitzgerald, "Divergence of the Kalman Filter," IEEE TAC 16(6):736, 1971 | 数值故障 |
§A1.24 与后续子任务的桥梁¶
本章建立的代数与统计机器将在后续 Kalman 族子任务中反复复用：
- → 5-A2（欧氏非线性滤波）：EKF 把 \((F, H)\) 换成 Jacobian \((\partial f/\partial x, \partial h/\partial x)\)；UKF/CKF/GHKF 把贝叶斯积分近似为求积点加权；FEJ 解决线性化一致性问题（直接对应本章 NEES 过自信诊断）；MSCKF 把 \(\chi^2\) gate 应用到滑窗视觉量测。
- → 5-A3（流形滤波）：ESKF 在 \(\mathfrak{so}(3)\) 切空间上跑本章 KF；MEKF/InEKF 重新定义状态误差以获得状态无关 Jacobian；UKF-M/IKFoM/EqF 把 sigma point/不变性搬到 Lie 群。所有变体的代数核心仍是协方差形 KF 更新 + Joseph form。
- → 5-A4（迭代变体 / RTS 平滑 / MHE / 工程映射）：IEKF 是单步 Gauss–Newton 迭代；RTS 是 KF 的前向-后向版（本章 DARE 的对偶时间方向）；MHE 用滑动窗口把滤波 MAP 化；全景对比表把本章的协方差/信息/平方根三轴拉伸为 9 种现代变体。
- → 5-B（因子图与非线性最小二乘）：本章"信息矩阵 = \(J^\top\Sigma^{-1}J\)"一语直接连到因子图后端的 Hessian；GTSAM/iSAM2 的 Bayes tree 就是稀疏 Cholesky 在增量场景的组织；RTS 是 smoother 的特例，因子图是平滑器的一般化。
一句话总结：本章的 KF + 信息形 + 平方根 + 一致性诊断**四件套，是把"递推贝叶斯"从**理论**变为**可上线航天器、可跑 100 Hz VIO、可诊断故障**的工程实体的全部基础机器。后续所有 Kalman 族变体都是在此框架上插入非线性算子、换代数空间、调窗口长度、改参数化方式而已——**地基稳了，上层万变不离其宗。¶
§A1.25 贝叶斯递推的逐行展开：从联合分布到在线滤波 ⭐⭐⭐¶
这一节解决什么问题：把 §A1.2 的两行贝叶斯递推拆成可以手推的每一步，明确每一步用了哪条概率规则。
在线滤波从一个事实开始：
机器人在第 \(k\) 时刻只允许使用 \(y_{1:k}\)。
未来量测 \(y_{k+1:T}\) 还没有发生，不能进入控制闭环。
因此目标不是完整轨迹后验 \(p(x_{0:T}\mid y_{1:T})\)，而是当前边缘后验：
如果直接从定义计算：
需要把历史状态 \(x_{0:k-1}\) 全部积分掉：
这个积分维度随时间增长。
若状态维度是 \(n\)，第 \(k\) 步积分维度是 \(kn\)。
这对实时机器人不可接受。
贝叶斯滤波的核心价值，就是利用两个条件独立假设把这个高维积分压缩成固定维度递推。
回顾概率链式法则：
把 \(a=x_k\)，\(b=x_{k-1}\)，\(c=y_{1:k-1}\)，得到：
马尔可夫假设说：
这句话的含义不是“历史不重要”。
历史已经通过 \(p(x_{k-1}\mid y_{1:k-1})\) 压缩进了上一步后验。
给定 \(x_{k-1}\) 后，更早的 \(y_{1:k-1}\) 不再直接影响 \(x_k\)。
于是边缘化 \(x_{k-1}\)：
这就是预测步。
它把“上一时刻后验”通过动力学传播成“当前时刻先验”。
新量测到来后，用 Bayes 公式：
观测条件独立假设说：
于是：
分母只是让概率积分为 1。
在滤波实现中，它也给出新息似然：
如果这个值极小，说明当前量测在模型下很不合理。
\(\chi^2\) gate、NIS 和异常量测剔除都来自这个位置。
本质洞察：预测步是“沿时间边缘化旧状态”，更新步是“用新量测重新加权当前状态”。Kalman filter 的所有矩阵公式只是这两步在高斯族中的闭式写法。
把这个递推与普通软件缓存类比：
| 软件缓存 | 贝叶斯滤波 |
|---|---|
| 上一次缓存状态 | \(p(x_{k-1}\mid y_{1:k-1})\) |
| 后台数据变化模型 | \(p(x_k\mid x_{k-1})\) |
| 刷新后的预测缓存 | \(p(x_k\mid y_{1:k-1})\) |
| 新请求返回的数据 | \(y_k\) |
| 根据新数据修正缓存 | \(p(x_k\mid y_{1:k})\) |
这个类比的边界也很重要。
缓存通常只保存一个值。
滤波器保存的是分布。
均值告诉控制器“当前最好估计是多少”。
协方差告诉控制器“这个估计有多可靠”。
如果不使用递推会怎样？
第一种做法是每来一帧都对全历史重做 batch 推断。
精度可能更高，但计算量随时间增长。
对 200 Hz IMU 和 30 Hz 相机的 VIO，这会很快超过实时预算。
第二种做法是只保留最后一次量测。
计算便宜，但丢弃历史信息。
机器人会在传感器遮挡、GPS 跳变或视觉短暂丢失时立即退化。
贝叶斯递推正好处在两者之间：
它用固定大小的后验分布压缩历史。
它又允许新量测持续改变信念。
练习¶
- 在草稿纸上从 \(p(x_k,x_{k-1}\mid y_{1:k-1})\) 出发，重写预测步的三行推导，并标注每一步使用“链式法则”“马尔可夫性”还是“边缘化”。
- 给出一个观测不满足条件独立的例子，例如相机自动曝光导致连续图像噪声相关，说明此时更新步需要怎样修改。
- 对一个一维离散状态系统手算两步贝叶斯滤波，状态取 \(\{0,1\}\)，转移概率和观测概率自定，观察归一化常数如何变化。
§A1.26 线性高斯预测的矩推导：为什么是 \(FPF^\top+Q\) ⭐⭐⭐¶
这一节解决什么问题：不直接背预测公式，而是从随机变量线性变换的均值和协方差一步步推出。
设上一时刻后验为：
线性动力学为：
过程噪声满足：
且 \(w\) 与 \(x_{k-1}\) 独立。
第一步求均值：
所以：
第二步写预测误差：
代入动力学和均值：
第三步求协方差：
因为 \(w\) 与 \(\tilde x_{k-1}\) 独立，且 \(\mathbb{E}[w]=0\)：
同理：
于是只剩两项：
如果过程噪声不是零均值，例如 \(\mathbb{E}[w]=\bar w\)，均值预测必须写成：
如果 \(w\) 与 \(x_{k-1}\) 相关，交叉项不能消失：
这解释了为什么教材总是反复声明“噪声独立”。
独立不是装饰条件。
它直接决定协方差传播是否只有两项。
物理直觉：
\(F P F^\top\) 是旧不确定性被动力学拉伸、旋转或压缩后的结果。
\(Q\) 是本步动力学新增的不确定性。
若机器人在湿滑地面上行驶，\(Q\) 应增大。
若使用高质量 IMU 且采样频率很高，短时间内 \(Q\) 可以较小。
但 \(Q\) 不能随意设为零。
设为零等价于声称运动模型完全正确。
真实系统中这几乎永远不成立。
连续白噪声到离散 \(Q_d\)¶
连续模型常写成：
其中：
离散状态转移：
离散过程噪声协方差：
若 \(A=0\)，则：
这就是工程里常见的“白噪声谱密度乘采样时间”。
若 \(A\neq0\)，直接用 \(Q_c\Delta t\) 是一阶近似。
采样周期很小、系统变化慢时可接受。
高速旋转的 IMU 姿态传播中，这个近似会变差。
练习¶
- 对常速度模型 \(x=[p,v]^\top\)，\(F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}\)，手推 \(FPF^\top\) 的四个元素，解释位置方差为什么会吸收速度方差。
- 假设加速度连续白噪声谱密度为 \(q_a\)，推导一维常速度模型的 \(Q_d=q_a\begin{bmatrix}\Delta t^3/3&\Delta t^2/2\\\Delta t^2/2&\Delta t\end{bmatrix}\)。再说明若把加速度看成每个采样周期内不变的离散随机变量 \(a_k\sim\mathcal N(0,\sigma_a^2)\)，为什么会得到 \(\sigma_a^2\begin{bmatrix}\Delta t^4/4&\Delta t^3/2\\\Delta t^3/2&\Delta t^2\end{bmatrix}\)。
- 说明把 \(Q\) 设为 0 会如何影响 NEES、NIS 和长期新息白噪声检验。
§A1.27 更新步的配方推导：从两个高斯相乘到 Kalman 增益 ⭐⭐⭐⭐¶
这一节解决什么问题：从后验密度的二次型出发，把信息形和协方差形的更新完整连接起来。
预测后先验为：
线性观测为：
量测似然为：
后验正比于二者乘积：
取负对数并丢掉与 \(x\) 无关的常数：
展开第一项：
展开第二项：
收集关于 \(x\) 的二次项：
收集一次项：
因此后验的信息矩阵和信息向量为：
这就是信息形更新。
现在把它变成常见的 Kalman 增益形式。
从信息形均值开始：
记：
则：
由于：
代入：
因此只要证明：
就得到标准更新。
这一步正是 Woodbury 恒等式的一个推论。
于是定义：
得到：
协方差也可由 Woodbury 得到：
即：
这个简化式在精确代数和最优 \(K\) 下成立。
在浮点数中建议使用 Joseph form：
Joseph form 的好处可以从误差写法直接看出。
更新误差为：
由于：
所以：
取协方差：
这对任意 \(K\) 都成立。
因此当 \(K\) 因数值误差、截断误差或人为调节不是精确最优时，Joseph form 仍有清楚的统计意义。
Kalman 增益的两个极限：
若 \(R\to\infty\)：
滤波器几乎忽略量测。
若 \(P^-\to\infty\) 且 \(H\) 满列可观：
滤波器主要相信量测。
这不是经验调权。
它是由先验不确定性和观测不确定性的相对大小自动决定的。
练习¶
- 对标量系统 \(x^-=10\)，\(P^-=4\)，\(H=1\)，\(R=1\)，\(y=12\)，手算 \(K,\hat x^+,P^+\)。
- 保持其他量不变，把 \(R\) 改成 100，再手算一次，解释为什么更新幅度变小。
- 证明 Joseph form 展开后在最优 \(K\) 下等于简化式。
§A1.28 信息形的几何意义与机器人多传感器融合 ⭐⭐⭐¶
这一节解决什么问题：把“信息矩阵相加”解释成多个独立量测对同一状态的约束叠加。
一个高斯因子可以写成：
线性残差：
展开二次项：
因此每个量测贡献：
多个条件独立量测相乘：
在负对数中变成求和：
这就是信息形“天然支持并行融合”的原因。
它像把多根弹簧同时拉在同一个质点上。
每根弹簧的刚度是 \(\Sigma_i^{-1}\)。
刚度越大，量测越可信，信息贡献越大。
弹簧方向由 \(H_i\) 决定。
如果某根弹簧只约束 \(x\) 方向，它不会提供 \(y\) 方向信息。
2D 机器人例子¶
状态：
GPS 量测位置：
对应：
电子罗盘量测航向：
对应：
若两者独立，更新信息为：
矩阵结构是近似块对角的：
如果再加入一个相机观测到的路标方位角，\(H\) 会同时含有位置和航向偏导。
信息矩阵就出现非对角块。
非对角块不是“耦合错误”。
它表示该量测把位置误差和航向误差绑在了一起。
这正是 SLAM 中协方差会变稠密、信息矩阵却保持局部稀疏的原因。
信息形的边界¶
信息形更新很漂亮，但预测不漂亮。
预测需要：
这意味着要回到协方差再求逆。
因此：
| 场景 | 更适合的形式 | 原因 |
|---|---|---|
| 高频 IMU 预测、低频量测 | 协方差形 | 预测便宜 |
| 多传感器同一时刻批量更新 | 信息形 | 更新可相加 |
| 大规模 SLAM 后端 | 信息形/平方根信息形 | 稀疏结构明显 |
| 低维嵌入式滤波 | 协方差形或 UD | 实现简单、时延可控 |
练习¶
- 对 GPS+罗盘例子，令 \(R_g=\mathrm{diag}(0.25,0.25)\)，\(R_c=0.01\)，写出 \(\Delta\Lambda\)。
- 解释为什么路标观测会让机器人位姿和路标位置之间出现信息矩阵非零块。
- 画一个 3 个位姿、2 个路标的小因子图，并标出对应信息矩阵中的非零块。
§A1.29 平方根形的逐步推导：为什么 QR 比显式求协方差稳 ⭐⭐⭐¶
这一节解决什么问题：从 \(P=SS^\top\) 出发，说明预测和更新为什么可以用 QR/Cholesky 修改完成。
协方差形直接传播：
如果 \(P=SS^\top\)，\(Q=GG^\top\)，则：
把两个矩阵横向拼起来：
则：
所以预测问题变成：
给定 \(A\)，找一个三角或稳定的平方根 \(S^-\)，使：
QR 分解正好完成这件事。
对 \(A^\top\) 做 QR：
其中 \(Q^\top Q=I\)。
于是：
取：
就得到预测平方根。
这个过程避免了直接形成 \(FPF^\top\)。
直接形成协方差会放大条件数。
平方根传播只处理条件数较低的因子。
平方根更新的观测视角¶
信息形式更新：
若：
且观测噪声白化后：
则后验最小二乘问题为：
对左侧堆叠矩阵做 QR。
新的上三角因子就是后验信息矩阵的平方根。
这就是 SRIF 与因子图后端之间的关系。
GTSAM 的稀疏 QR/Cholesky 可以看成大规模平方根信息滤波的批量版本。
工程检查清单¶
| 检查项 | 协方差形风险 | 平方根形处理 |
|---|---|---|
| 对称性 | \(P\) 可能因舍入不对称 | \(SS^\top\) 天然对称 |
| 正定性 | 减法可能产生负特征值 | QR 保持平方和结构 |
| 条件数 | \(\mathrm{cond}(P)\) 大 | \(\mathrm{cond}(S)=\sqrt{\mathrm{cond}(P)}\) |
| 白化残差 | 常被忽略 | SRIF 中显式处理 |
| 稀疏性 | 协方差常稠密 | 信息平方根保留图结构 |
练习¶
- 设 \(P=\begin{bmatrix}4&0\\0&1\end{bmatrix}\)，\(F=\begin{bmatrix}1&1\\0&1\end{bmatrix}\)，\(Q=0.01I\)，手算 \(A=[FS,G]\) 的结构。
- 用任意数值工具验证 \(AA^\top=FPF^\top+Q\)。
- 说明为什么 SRIF 更像因子图优化，而 Potter/UD 更像传统递推滤波。
§A1.30 机器人状态估计案例：2D 移动机器人融合轮速计、IMU 与 GPS ⭐⭐⭐¶
这一节解决什么问题：用一个低维但完整的机器人例子，把预测、更新、信息量、调参与诊断串起来。
考虑差速移动机器人。
状态选为：
其中 \(p_x,p_y\) 是世界系位置。
\(\theta\) 是航向角。
\(v\) 是前向速度。
\(\omega\) 是 yaw rate。
控制输入来自轮速计：
一个简单离散模型为：
这个模型其实是非线性的，因为有 \(\sin\theta,\cos\theta\)。
如果只在小角度附近使用，可以线性化成 A2 的 EKF。
为了在 A1 中保持线性高斯，我们先固定 \(\theta\) 的线性化点 \(\bar\theta\)，得到局部线性模型：
其中：
这里的 \(F_k\) 已经显示出 A1 与 A2 的边界。
只要 \(F_k\) 是在某个点求出的 Jacobian，就已经进入 EKF 思想。
但一旦线性化完成，后续预测更新仍完全使用 A1 的 KF 机器。
GPS 量测：
IMU yaw rate 量测：
如果 GPS 丢失 5 秒，预测仍会依靠轮速计和 IMU 前进。
但位置协方差会持续增长。
GPS 恢复时，Kalman 增益会变大，因为 \(P^-\) 已经增大。
这就是“传感器回来后滤波器自动重新信任它”的机制。
如果 GPS 出现跳变，NIS 会突然变大。
这时不能简单更新，否则状态会被拉飞。
应先计算：
若：
则拒绝该 GPS 量测。
最小 Eigen 实现骨架¶
#include <Eigen/Dense>
struct LinearFusion2D {
Eigen::VectorXd x; // 状态: px, py, theta, v, yaw_rate
Eigen::MatrixXd P; // 状态协方差
Eigen::MatrixXd Q; // 过程噪声协方差
Eigen::Matrix2d R_gps; // GPS 位置噪声
double R_yaw_rate; // IMU yaw rate 噪声
void predict(const Eigen::MatrixXd& F, const Eigen::VectorXd& u) {
// 预测均值：状态必须和协方差一起向前传播
x = F * x + u;
// 预测协方差：旧不确定性被 F 传播，并叠加过程噪声
P = F * P * F.transpose() + Q;
// 数值对称化：消除浮点乘法带来的微小非对称
P = 0.5 * (P + P.transpose());
}
bool updateGps(const Eigen::Vector2d& z, double chi2_threshold) {
Eigen::Matrix<double, 2, 5> H;
H.setZero();
H(0, 0) = 1.0;
H(1, 1) = 1.0;
// 新息：真实量测与预测量测之差
Eigen::Vector2d r = z - H * x;
Eigen::Matrix2d S = H * P * H.transpose() + R_gps;
// NIS：用新息协方差归一化后的马氏距离平方
double nis = r.transpose() * S.llt().solve(r);
if (nis > chi2_threshold) {
return false; // 量测统计上不可信，拒绝更新
}
// 求解 Kalman 增益，避免显式 inverse
Eigen::Matrix<double, 5, 2> K = (S.llt().solve(H * P)).transpose();
x = x + K * r;
// Joseph form：保持协方差半正定
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(5, 5);
Eigen::MatrixXd IKH = I - K * H;
P = IKH * P * IKH.transpose() + K * R_gps * K.transpose();
P = 0.5 * (P + P.transpose());
return true;
}
};
这段代码体现了四个工程习惯：
第一，永远用 solve()，不要用 inverse()。
第二，每次更新后对称化 \(P\)。
第三，外部量测先过 NIS gate。
第四，协方差更新默认使用 Joseph form。
调参顺序¶
| 顺序 | 调什么 | 观察什么 |
|---|---|---|
| 1 | 量测噪声 \(R\) | 静止数据的量测方差 |
| 2 | 过程噪声 \(Q\) | 新息是否白、NEES 是否过上界 |
| 3 | 初始协方差 \(P_0\) | 初始收敛速度和是否锁死 |
| 4 | \(\chi^2\) gate 倍率 | 外点拒绝率和好量测误拒率 |
练习¶
- 把上述状态改成 \([p_x,p_y,v_x,v_y]^\top\) 常速度模型，写出 \(F,H,Q\)。
- 设计一个 GPS 跳变场景，计算跳变前后 NIS 的数量级差异。
- 在没有 GPS 的 10 秒内，预测位置方差应当单调增大还是减小？用公式解释。
本章常见误解汇总¶
| 误解 | 正确理解 |
|---|---|
| Kalman 滤波只适用于线性系统 | KF 本身要求线性高斯，但 Kalman "族"通过 EKF/UKF/CKF 等近似策略已扩展到非线性系统；线性高斯 KF 是整个族的理论基石而非全部 |
| Kalman 增益 \(K\) 是一个需要手动调节的超参数 | \(K\) 由 \(P\)、\(H\)、\(R\) 严格推导而来，是贝叶斯最优的结果；需要调的是过程噪声 \(Q\) 和观测噪声 \(R\)，而非 \(K\) 本身 |
| 协方差矩阵 \(P\) 越小说明估计越好 | \(P\) 小只说明滤波器"自信"，不代表估计真的准；如果 NEES 持续超上界，说明 \(P\) 过度自信（一致性破坏），此时小 \(P\) 反而是病态信号 |
| 信息形 KF 和协方差形 KF 是两种不同的滤波器 | 二者在代数上严格等价（通过 Woodbury 恒等式互换），只是数值性态和工程适用场景不同：信息形天然支持多传感器加法融合与稀疏 SLAM |
| 平方根滤波只是为了"加速计算" | 平方根形的核心优势是**数值稳定性**（条件数减半、保正定），而非速度；Apollo 登月选用它正是因为有限字长下协方差形会丢失正定性 |
| NEES 通过了就说明滤波器完全正确 | NEES 只检验"协方差是否与真实误差统计一致"，不检验估计均值是否有系统偏差；一致但有偏的滤波器 NEES 可能合格但估计轨迹偏离真值 |
| 过程噪声 \(Q\) 应该设得尽量小以获得"精确"预测 | \(Q\) 过小会让滤波器过度相信模型、忽视观测，导致协方差过度自信；\(Q\) 的物理含义是对模型不确定性的承认，而非"噪声越小模型越好" |
| \(\chi^2\) gate 阈值越严越好 | 阈值过严会误拒正常观测（尤其在高维状态下 \(\chi^2\) 分布的尾部较宽），导致可用观测不足、估计退化；需要根据自由度和可接受的误拒率合理设定 |
§A1.31 常见故障排查手册 ⭐⭐¶
| 症状 | 可能原因 | 排查步骤 | 对应公式 |
|---|---|---|---|
| Cholesky 分解失败 | \(P\) 非对称或非正定 | 先对称化，再看最小特征值；若仍失败，检查 Joseph form 和 \(Q,R\) 是否 SPD | \(P^+=(I-KH)P(I-KH)^\top+KRK^\top\) |
| 估计长期滞后 | \(R\) 过大或 \(Q\) 过小 | 画 Kalman 增益与新息；增大 \(Q\) 或减小可信传感器的 \(R\) | \(K=P H^\top(HPH^\top+R)^{-1}\) |
| 状态被单帧 GPS 拉飞 | 缺少 NIS gate 或 \(R_g\) 设得太小 | 打印 \(\epsilon=\tilde y^\top S^{-1}\tilde y\)；超过阈值时拒绝量测 | \(\epsilon\sim\chi^2_m\) |
| NEES 持续超上界 | 协方差过自信 | 增大过程噪声、检查线性化误差、检查漏建 bias | \(\varepsilon=(x-\hat x)^\top P^{-1}(x-\hat x)\) |
| NIS 持续低于下界 | 协方差过保守或重复计量 | 检查同一传感器是否被融合两次；检查 \(R\) 是否过大 | \(\mathbb{E}[\epsilon]=m\) |
| 新息存在周期性 | 模型漏掉周期扰动 | 对新息做自相关和频谱；加入轮胎周期误差或 IMU bias 状态 | whiteness test |
| 信息形求均值很慢 | \(\Lambda\) 稀疏但直接求逆 | 用稀疏 Cholesky 解 \(\Lambda x=\eta\)，不要显式求逆 | \(\eta=\Lambda\hat x\) |
排查顺序建议如下：
- 先看数值合法性：\(P=P^\top\)、特征值非负、\(Q,R\) 单位正确。
- 再看统计一致性：NEES/NIS 是否在合理区间。
- 再看时间结构：新息是否白、是否有延迟或周期相关。
- 最后看建模边界：线性模型是否已经无法覆盖真实运动。
§A1.32 本章新增累积项目：Mini-State-Estimator 的线性模块 ⭐⭐¶
本批数学方向的累积项目可以命名为 Mini-State-Estimator。
A1 负责实现最小线性高斯模块。
模块接口建议如下：
class LinearKalmanFilter {
public:
void SetState(const Eigen::VectorXd& x, const Eigen::MatrixXd& P);
void Predict(const Eigen::MatrixXd& F,
const Eigen::VectorXd& b,
const Eigen::MatrixXd& Q);
bool Update(const Eigen::VectorXd& z,
const Eigen::MatrixXd& H,
const Eigen::MatrixXd& R,
double chi2_threshold);
double LastNIS() const;
private:
Eigen::VectorXd x_; // 当前状态均值
Eigen::MatrixXd P_; // 当前状态协方差
double last_nis_ = 0.0; // 最近一次量测的 NIS
};
实现时必须满足：
Predict不修改量测相关变量。Update内部使用LLT或LDLT的solve()。Update先算 NIS，再决定是否更新。- 协方差更新使用 Joseph form。
- 每次更新后执行对称化。
- 单元测试覆盖标量 KF、二维位置 KF 和拒绝异常量测三种情况。
推荐单元测试：
| 测试 | 输入 | 期望 |
|---|---|---|
| 标量退化 | \(F=H=1\) | 与手算 \(K=P/(P+R)\) 一致 |
| 零量测噪声极限 | \(R=10^{-9}\) | 更新后状态接近量测 |
| 大量测噪声极限 | \(R=10^9\) | 更新后状态几乎不变 |
| 异常 GPS | NIS 超阈值 | Update 返回 false |
| 正定性 | 随机 SPD 矩阵 | 更新后最小特征值非负 |
§A1.33 本章小结补充：一张表串起所有基础机器 ⭐⭐¶
| 机器 | 解决的问题 | 核心公式 | 工程关注点 |
|---|---|---|---|
| 贝叶斯递推 | 在线维护后验 | 预测积分 + Bayes 更新 | 条件独立假设是否成立 |
| 线性高斯预测 | 动力学传播不确定性 | \(P^-=FPF^\top+Q\) | \(Q_d\) 离散化与单位 |
| 线性高斯更新 | 用量测修正先验 | \(K=P^-H^\top S^{-1}\) | 不显式求逆 |
| Joseph form | 保持协方差稳定 | \(P^+=(I-KH)P^-(I-KH)^\top+KRK^\top\) | float32 和长时间运行必用 |
| 信息形 | 多量测并行融合 | \(\Lambda^+=\Lambda^-+H^\top R^{-1}H\) | 预测较贵，后端稀疏 |
| 平方根形 | 降低条件数 | \(P=SS^\top\) | QR/UD/SRIF 选型 |
| NEES | 仿真一致性 | \((x-\hat x)^\top P^{-1}(x-\hat x)\) | 需要真值 |
| NIS | 在线一致性 | \(\tilde y^\top S^{-1}\tilde y\) | 可直接用于 gate |
| 白噪检验 | 检测模型偏差 | 新息自相关 | 发现延迟和漏建状态 |
延伸阅读¶
| 类型 | 资源 | 难度 | 重点 |
|---|---|---|---|
| 教材 | Särkkä & Svensson, Bayesian Filtering and Smoothing 2nd ed. (2023), Ch.4–6 | ⭐⭐ | 贝叶斯递推框架与线性高斯滤波的标准参考 |
| 教材 | Simon, Optimal State Estimation (2006), Ch.5–8 | ⭐⭐ | KF 推导、稳态、平方根形、信息形 |
| 教材 | Bar-Shalom et al., Estimation with Applications to Tracking and Navigation (2001), Ch.5 | ⭐⭐⭐ | NEES/NIS 一致性诊断的权威出处 |
| 教材 | Barfoot, State Estimation for Robotics 2nd ed. (2024), Ch.3–4 | ⭐⭐ | 从概率到批量/递推估计的统一框架 |
| 论文 | Kalman, "A New Approach to Linear Filtering and Prediction Problems", ASME J. Basic Eng. 82(1):35–45, 1960 | ⭐⭐⭐ | 原始论文，历史价值 |
| 论文 | Bierman, Factorization Methods for Discrete Sequential Estimation (1977) | ⭐⭐⭐ | UD 分解与平方根滤波的工程圣经 |
| 论文 | Maybeck, Stochastic Models, Estimation, and Control Vol.1 (1979), Ch.1–7 | ⭐⭐⭐ | 航空航天 KF 理论最经典教材 |
| 代码 | cra-ros-pkg/robot_localization |
⭐⭐ | ROS 社区 EKF/UKF 工业标准实现 |
| 代码 | MATLAB kalmanFilter / Sensor Fusion Toolbox |
⭐ | 快速验证公式正确性 |
§A1.34 仍需带到 A2 的问题 ⭐⭐¶
A1 建立在线性高斯世界中。
这个世界足够重要，因为所有现代滤波器都会在局部退化成它。
但真实机器人马上会打破三个假设：
第一，动力学非线性。
IMU 姿态传播包含旋转矩阵和指数映射。
轮式机器人位置传播包含 \(\sin\theta,\cos\theta\)。
腿式机器人接触运动学包含足端约束切换。
第二，观测非线性。
相机像素观测包含透视投影。
激光匹配残差包含最近邻和局部平面。
UWB 距离观测是欧氏距离范数。
第三，状态空间非欧氏。
姿态不属于普通向量空间。
位姿也不是简单的六维向量加法。
A2 先处理前两个问题：
用 EKF 的 Taylor 展开近似非线性函数。
用 UKF/CKF/GHKF 的确定性采样近似非线性积分。
A3 再处理第三个问题：
把误差定义在切空间或李群上，而不是直接把姿态当普通向量相加。
因此阅读 A2 时要始终记住：
A2 的每个非线性滤波器，在完成近似后，都会回到 A1 的更新骨架。
Kalman 增益、Joseph form、NIS、NEES、信息矩阵、平方根稳定性仍然是同一套基础机器。
常见陷阱与故障排查¶
⚠️ 陷阱一：把连续谱密度当成离散方差。 连续白噪声积分会产生 \(\Delta t^3/3\)、\(\Delta t^2/2\)、\(\Delta t\)；离散常加速度样本才会产生 \(\Delta t^4/4\)、\(\Delta t^3/2\)、\(\Delta t^2\)。
⚠️ 陷阱二：只传播协方差不传播均值。 \(P\) 变大只能描述不确定性，不能替代 \(x^-=Fx+u\)；均值停在旧时刻会让所有量测残差带系统偏差。
⚠️ 陷阱三：把 NIS gate 当作万能外点检测。 NIS 只检验当前量测是否符合滤波器假设；若 \(Q/R\) 本身调错，gate 也会跟着失真。
| 故障排查现象 | 可能原因 | 处理方式 |
|---|---|---|
| NEES 长期高于置信上界 | \(Q\) 偏小或模型漏项 | 增大过程噪声，检查离散化单位 |
| NIS 大量拒绝好量测 | \(R\) 偏小或时间同步错误 | 用静态数据估计 \(R\)，检查时间戳 |
| \(P\) 出现非对称或负特征值 | 直接协方差更新数值不稳 | 使用 Joseph form 或平方根滤波 |
本质洞察：Kalman 滤波不是“调一个增益”，而是在模型预测的信息和量测带来的信息之间做概率加权；所有调参最终都回到 \(Q\)、\(R\)、\(P\) 的单位和可信度。
练习¶
- 对一维常速度模型分别从连续白噪声谱密度和离散加速度样本两种假设推导 \(Q_d\)，比较 \(\Delta t\) 次幂差异。
- 写一个只传播 \(P\) 不传播 \(x\) 的小例子，观察 GPS residual 如何随时间系统性增大。
- 用同一段数据画出 NIS 序列，调整 \(R\) 直到约 95% 的二维 GPS 新息低于 \(\chi^2_{2,0.95}\)。