20_经典非线性滤波族

博士前数学路线图·第五批·子专题 A2：经典非线性滤波族——EKF、UKF、CKF 与 GHKF¶

底线陈述（BLUF）：A1 建立的线性高斯 KF 是"最优但不实用"的理想模型——真实机器人的动力学与观测几乎全部非线性。本专题讲透四种经典近似策略：EKF（一阶 Taylor 线性化，$O(n^3)$，工业界默认）、UKF（Unscented Transform sigma 点传播，二阶均值精度，免 Jacobian）、CKF（球面径向 Cubature 积分，是 Scaled UT 的特例，高维更稳定）、GHKF（Gauss-Hermite 积分，任意精度但维度诅咒）。更重要的是，本专题揭示 EKF 在 SLAM/VIO 中的**一致性病理**——线性化点依赖估计导致虚假可观测性与协方差过度自信（Huang-Mourikis-Roumeliotis 2008-2010），以及 FEJ/OC-EKF 等修复方案。理解这些病理是进入 A3（流形滤波/InEKF 的结构性解决方案）的必要前提。

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回 A1 复习

编号	问题	答不出时回顾
1	写出线性 KF 的预测和更新公式（含 Joseph form）。Kalman 增益 $K$ 的两个极限行为是什么？	A1 §A1.5
2	信息矩阵 $\Lambda=P^{-1}$ 的更新公式为什么只有加法？解释其工程意义。	A1 §A1.9-A1.10
3	NEES 的定义是什么？$\varepsilon_k\sim\chi^2_n$ 成立的前提条件是什么？"超出上界"意味着什么？	A1 §A1.15-A1.16
4	Woodbury 恒等式如何把信息形 KF 变换为协方差形 KF？写出关键步骤。	A1 §A1.7
5	平方根滤波器为什么能"翻倍有效精度"？写出条件数关系式。	A1 §A1.12

本章目标¶

学完本章后，你应该能够：

推导 EKF 的完整预测-更新公式，明确哪一步做了 Taylor 近似
解释 EKF 在 SLAM/VIO 中的一致性病理（虚假可观测性），并说明 FEJ/OC-EKF 的修复原理
推导 Unscented Transform 的 sigma 点生成与权重匹配，证明其三阶精度
对比 EKF、UKF、CKF、GHKF 的近似对象、精度阶、点数与适用场景
定位 OpenVINS 源码中 MSCKF 更新、FEJ 开关、null-space 投影的实现位置

为什么你必须学这个专题¶

如果你的博士目标是 SLAM、RL-based 运动控制或具身智能，那么 EKF / UKF / CKF 这三个名字就是你进入现代 VIO 工程栈的门票。MSCKF（Mourikis-Roumeliotis 2007）、OpenVINS（Geneva et al. 2020）、ROVIO（Bloesch et al. 2015）、早期 VINS-Mono 的 IMU 预积分协方差递推、S-MSCKF、LARVIO、Google ARCore 早期的 6-DoF tracker，都是**一阶线性化 EKF 家族**。ROS 社区从 2014 年起就把 robot_localization 的 Ekf / Ukf 作为机器人里程计融合的工业事实标准；MATLAB 的 Sensor Fusion Toolbox、AirSim 的姿态估计、PX4 EKF2 无人机飞控主回路，亦无例外。

但更重要的是：不理解 EKF 的一致性病理（Huang-Mourikis-Roumeliotis 一系列工作），你就读不懂 FEJ、OC-EKF、IEKF、MSCKF 2.0、Invariant EKF 这些过去十年机器人估计领域最重要的一条论文线——这条线直接通向 Barrau-Bonnabel 的不变滤波、Brossard 的 UKF-on-Manifolds，以及下一批（5-A3）流形滤波的全部内容。欧氏空间的 EKF/UKF 是本批最硬的基本功：它不优雅，不优美，甚至在 SLAM 上是错的；但你必须先理解它为什么错，才能理解流形滤波为什么对。

这一子专题严格对标 Barfoot 2ed §4.2、Särkkä & Svensson 2ed Ch.7–8、Thrun et al. Ch.3/7/10、Simon 2006 Ch.13–14，并结合 MSCKF/OpenVINS 的源码展开。学完后你应该能：(1) 徒手推导 EKF 和 UKF 的完整预测-更新；(2) 解释 FEJ 为什么能修复 EKF-SLAM 的偏航过自信；(3) 证明 CKF 是 Scaled UT 的特例；(4) 在 OpenVINS 源码里定位到 UpdaterMSCKF::nullspace_project_inplace。

§A2.1 从 KF 到非线性：EKF 的一阶线性化 ⭐⭐¶

动机与定位¶

线性 Kalman Filter（5-A1 已讲）在 $x_{k}=F_{k-1}x_{k-1}+w_{k-1},\,y_k=H_kx_k+v_k$，$w,v$ 高斯白噪声的前提下是严格最优 MMSE 估计器。真实机器人从来不满足这个前提：IMU 角速度积分、相机投影 $h(x)=K\pi(Rp+t)$、轮式里程计的 $\sin\theta,\cos\theta$，全部是非线性。Extended Kalman Filter（EKF）的**唯一动作**是：在当前估计点对 $f$ 和 $h$ 做一阶 Taylor 展开，把非线性系统局部当作线性系统，然后继续沿用 KF 的公式。它**不是贝叶斯最优**，它损失了 Taylor 二阶及以上的所有项；但它便宜、递推、$O(n^3)$，并且在非线性不强、初值较准的情况下能用。

一阶 Taylor 线性化¶

给定非线性状态方程 $x_k = f(x_{k-1}, u_{k-1}) + w_{k-1},\, w\sim\mathcal{N}(0,Q)$ 和观测方程 $y_k = h(x_k) + v_k,\, v\sim\mathcal{N}(0,R)$，EKF 在当前 mean 处展开：

\[ f(x_{k-1}) \approx f(\hat{x}_{k-1|k-1}) + F_{k-1}\,(x_{k-1}-\hat{x}_{k-1|k-1}),\quad F_{k-1}\triangleq \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k-1|k-1}} \]

\[ h(x_k)\approx h(\hat{x}_{k|k-1}) + H_k\,(x_k-\hat{x}_{k|k-1}),\quad H_k\triangleq \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k|k-1}} \]

关键事实：EKF 的 Jacobian 每一步都在当前状态估计处重新计算。这个"每一步重新线性化"正是后面一致性问题的全部根源。

为什么 EKF 不是最优贝叶斯滤波器¶

真实的贝叶斯递推 $p(x_k|y_{1:k}) \propto p(y_k|x_k)\int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1}$ 在非线性 $f,h$ 下一般**不再是高斯**。EKF 强行把它当作高斯，且只用一阶 Taylor。把非线性 $g(x)$ 对 $x\sim\mathcal{N}(\mu,P)$ 做精确期望：

\[ \mathbb{E}[g(x)] = g(\mu) + \tfrac{1}{2}\mathrm{tr}(\nabla^2 g\cdot P) + O(P^2) \]

EKF 直接取 $g(\mu)$，丢弃了 Hessian 项 $\tfrac{1}{2}\mathrm{tr}(\nabla^2 g\cdot P)$——这就是二阶偏差。当 $P$ 很大（初始化期）或 $g$ 的曲率很大（例如接近奇异姿态），这个偏差会被不断递推放大，直到滤波器发散。

§A2.2 EKF 的严格推导与算法流程 ⭐⭐¶

预测步¶

\[ \boxed{\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1}, u_{k-1})} \]

\[ \boxed{P_{k|k-1} = F_{k-1}\,P_{k-1|k-1}\,F_{k-1}^\top + Q_{k-1}} \]

更新步¶

计算 innovation $\tilde{y}_k = y_k - h(\hat{x}_{k|k-1})$ 与其协方差 $S_k = H_k P_{k|k-1} H_k^\top + R_k$，Kalman 增益 $K_k = P_{k|k-1}H_k^\top S_k^{-1}$，则

\[ \boxed{\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k\,\tilde{y}_k} \]

\[ \boxed{P_{k|k} = (I-K_kH_k)\,P_{k|k-1}\,(I-K_kH_k)^\top + K_kR_kK_k^\top \quad \text{(Joseph form)}} \]

严格使用 Joseph form：它对 $K$ 的数值扰动是半正定保持的；若先验、量测噪声和可观条件足够强，才进一步表现为正定保持。常见的化简版 $P_{k|k}=(I-K_kH_k)P_{k|k-1}$ 一旦 $K$ 偏离最优就会失去这种结构（Bucy-Joseph 1968；Simon 2006 §6.2）。

教材交叉定位¶

教材	EKF 位置	二阶 EKF 位置	IEKF 位置
Barfoot 2ed (2024)	§4.2.3 p.114	无专节（§4.2.7 概论）	§4.2.5 p.119；§4.2.6 证明 IEKF=MAP
Särkkä & Svensson 2ed (2023)	Ch.7.2 p.113	§7.3 p.119 Higher-Order EKF	§7.4 p.121；§7.5 LM/Line-search IEKF
Thrun et al. (2005)	§3.3.2 Table 3.3 p.50	未涉及	未涉及
Simon (2006)	§13.4 discrete EKF	§13.6-13.7 Second-order EKF	§13.5 IEKF
Probabilistic Robotics (2005)	§3.3 定位；Ch.10.2 EKF-SLAM Table 10.1/10.2	—	—

（重要勘误：Särkkä 2ed 的 Sigma-point 家族在 Ch.8 General Gaussian Filtering，不在原始 1st ed 的 Ch.6；EKF-SLAM 在 Thrun 书的 Ch.10 而非 Ch.7。Simon 2006 的二阶 EKF 是 §13.6-13.7，不是 §13.3。）

算法伪码（Thrun Table 3.3 结构，按本文 $Q/R$ 约定改写）¶

Thrun 原书常用 $R_t$ 表示运动噪声、$Q_t$ 表示观测噪声；本文全专题统一采用 $Q_t$ 表示过程噪声、$R_t$ 表示观测噪声。读论文或迁移代码时，先核对符号表，再抄公式。

EKF(μ_{t-1}, Σ_{t-1}, u_t, z_t):
  μ̄_t  = g(u_t, μ_{t-1})                       # 预测均值
  Σ̄_t  = G_t Σ_{t-1} G_t^⊤ + Q_t                # 预测协方差,  G_t = ∂g/∂x|_{μ_{t-1}}
  K_t  = Σ̄_t H_t^⊤ (H_t Σ̄_t H_t^⊤ + R_t)^{-1}   #  H_t = ∂h/∂x|_{μ̄_t}
  μ_t  = μ̄_t + K_t (z_t - h(μ̄_t))                # 更新均值
  Σ_t  = (I - K_t H_t) Σ̄_t                       # (数值稳健时换 Joseph form)
  return (μ_t, Σ_t)

EKF 为什么会发散——几何直觉¶

EKF 的线性化平面是**当前估计的切平面**。如果 $\hat{x}$ 本身有偏差，切平面选错了位置，传播协方差就系统性地偏离真实后验。更坏的是：线性化下的 $H_k$ 可能把本来**不可观测**的方向（如 SLAM 的全局偏航）错误地"看作可观测"，于是 Kalman 增益沿着虚假方向吸收信息，导致协方差在这些方向上假性收缩——一旦协方差变小、增益变大，滤波器就把每次小观测噪声都当真，估计迅速偏离。这就是下一节要深入的**一致性灾难**。

⚠️ 思维陷阱：认为 EKF 发散可以通过调大 $Q$ 来根治。 增大 $Q$ 确实能让协方差不会过度收缩，但这是"用保守性换稳定性"的权宜之计——估计精度也会下降。更本质的问题（如不可观测方向的虚假信息获取）不是 $Q$ 能解决的，需要 FEJ/OC-EKF/InEKF 等结构性修正。

练习¶

对一维非线性观测 $h(x)=x^3$ 在 $\hat{x}=1$ 处求 Jacobian $H$，手算 EKF 更新。再在 $\hat{x}=0.5$ 处求 $H$，比较两次更新的 Kalman 增益差异，解释"线性化点不同导致增益不同"。
写出 EKF 预测步使用 $f(\hat{x})$ 而非 $F\hat{x}$ 的原因（提示：均值传播是非线性的，协方差传播才是线性化的）。
对二维旋转 $f(x)=R(\omega\Delta t)x$，当 $\omega\Delta t=\pi/2$（90 度旋转）时 EKF 一阶近似有多大误差？用中心差分估算。

§A2.3 EKF Jacobian 的三种计算方式 ⭐⭐¶

Jacobian 的质量决定 EKF 的命运。工程上有三条路：

(1) 解析 Jacobian：最快、最准，但容易写错。对机器人本体动力学、IMU 运动学、相机投影这类有限规模的模型，强烈推荐手推并单元测试。调试方法：随机选点用中心差分验证——若相对误差 $>10^{-4}$，八成符号或指标写错了。代表：OpenVINS UpdaterHelper::get_feature_jacobian_full、VINS-Mono projection_factor.cpp 中的 jacobians[i]。

(2) 数值微分（Finite Difference）：中心差分 $\partial f/\partial x_i \approx [f(x+he_i)-f(x-he_i)]/(2h)$，最优步长 $h^\star\approx \varepsilon_{\text{mach}}^{1/3}\cdot\max(|x_i|,\text{scale})$。双精度下 $\varepsilon_{\text{mach}}\approx 2.22\times 10^{-16}$，故 $h^\star\approx 6\times 10^{-6}$。前向差分步长是 $\varepsilon^{1/2}\approx 1.5\times 10^{-8}$。参考：Ceres 文档 ceres-solver.org/numerical_derivatives.html；Nocedal & Wright Numerical Optimization 2ed §8.1。陷阱：对角度、四元数等流形变量直接差分会跨越奇点；必须用流形上的扰动（$\boxplus,\boxminus$）。

(3) 自动微分（AutoDiff）： - C++：Ceres Jet（ceres-solver/include/ceres/jet.h）是最成熟的前向模式实现，ceres::Jet<T,N> 把 double 扩展为对偶数；CppAD（coin-or/CppAD）；autodiff（autodiff.github.io，header-only，autodiff::dual）；Eigen 自带 AutoDiffScalar。 - Python：JAX 的 jax.jacfwd（前向，宽 Jacobian）与 jax.jacrev（反向，高状态）；PyTorch 的 torch.func.jacfwd/jacrev 与 torch.autograd.functional.jacobian；SymPy sympy.Matrix(f).jacobian(x) 用于推导期。

经验：解析 + 自动微分交叉验证，禁用纯数值微分作为生产路径。数值微分只做单元测试。

§A2.4 EKF 一致性问题：不可观测方向与虚假信息 ⭐⭐⭐¶

什么是"一致性"¶

一个估计器 $(\hat{x},P)$ 是一致的（consistent），当且仅当： 1. $\mathbb{E}[x-\hat{x}]=0$（无偏）； 2. $\mathbb{E}[(x-\hat{x})(x-\hat{x})^\top]\preceq P$（协方差不过自信）。

诊断工具是 NEES：$\varepsilon_k = (x_k-\hat{x}_k)^\top P_k^{-1}(x_k-\hat{x}_k)$。若误差近似高斯且真实误差协方差恰为 $P_k$，则 $\varepsilon_k\sim\chi^2_n$；若估计器只是保守（真实协方差 $\prec P_k$），NEES 会系统性偏低，不能再严格服从标准 $\chi^2_n$。$\chi^2$ 95% 双侧区间：$n=3$ 时为 $[\chi^2_{3,0.025},\chi^2_{3,0.975}]=[0.216,9.348]$；若用单侧上界 5% 则为 7.815。$N$ 次 Monte Carlo 平均：$\bar\varepsilon_k\in[\tfrac{1}{N}\chi^2_{Nn,0.025},\tfrac{1}{N}\chi^2_{Nn,0.975}]$；$N=50,n=3$ 时约为 $[2.36,3.72]$（Bar-Shalom 2001 §5.4.2）。超出上界 = 过自信 = 滤波器在撒谎；这是 EKF-SLAM 最常见的病态。

EKF-SLAM 的"虚假信息获取"¶

Huang, Mourikis, Roumeliotis (ICRA 2008, DOI 10.1109/ROBOT.2008.4543252) 首次严格证明：真实的 2D SLAM 非线性系统有 3 维不可观测子空间（全局 $x,y,\psi$）；但标准 EKF-SLAM 在每步重新线性化后，其线性化系统的不可观测子空间**只有 2 维**。论文原话："the standard EKF-SLAM employs an error-state system model that has an unobservable subspace of dimension two, even though the underlying nonlinear [system has three]"（§II.B）。

这一维"失踪"的不可观测方向，正是全局偏航 $\psi$。EKF 的线性化把真实不可观的偏航方向"数值上"变成了可观测——于是 Kalman 更新沿着这个虚假方向持续吸收信息，偏航协方差过早收敛，一旦收敛后再有小观测误差就被 Kalman 增益放大成估计偏移。

3D 情况下，纯 3D SLAM 的不可观测子空间是 6 维（3 平移 + 3 旋转）；VIO 因有重力方向约束 roll/pitch 可观，不可观测子空间降到 4 维（$x,y,z,\psi$）——见 Hesch, Kottas, Bowman, Roumeliotis "Consistency Analysis and Improvement of Vision-aided Inertial Navigation" IEEE T-RO 30(1), 2014，DOI 10.1109/TRO.2013.2277549。

几何直觉¶

想象在无 GPS 无地图先验的纯 SLAM 中把整个世界刚体地平移并绕重力轴旋转——所有传感器观测不变（相对几何保持）。所以这 3 或 4 维变换应对估计完全不可观测，协方差沿这些方向**永远**不应收缩。但 EKF 每步把 Jacobian 在不同点求值，相当于每次用不同的局部线性系统，不同时刻的不可观测子空间相互不对齐——叠加的信息矩阵把这些本应为零的方向"累加出"非零信息。Huang 2010 IJRR §4 的信息增益公式 $\Delta I = H_k^\top R_k^{-1} H_k$ 严格量化了这个虚假增益。

如果 EKF-SLAM 的偏航过自信不被修正会怎样？ 实际后果分三阶段。(1) 初期：偏航协方差 $P_{\psi\psi}$ 看似正常地缓慢收缩，因为每步虚假信息增量很小。(2) 中期：$P_{\psi\psi}$ 收缩到远小于真实偏航不确定性的水平，Kalman 增益开始系统性地沿偏航方向吸收噪声。(3) 末期：偏航估计被拉偏，所有依赖偏航的位置估计和路标估计一起偏移——整个地图开始"旋转"。在 EuRoC 数据集上关闭 FEJ 可以在 2 分钟内观察到 NEES 飞出 $\chi^2$ 上界 5-10 倍。

练习¶

对 2D SLAM（状态 $[x,y,\theta,m_1^x,m_1^y,\dots]$），写出全局平移和全局旋转变换下状态的变化形式，验证观测 $h(x)$ 在该变换下不变。
解释为什么 VIO 的不可观测子空间是 4 维（$x,y,z,\psi$）而非 6 维——重力方向如何约束了 roll 和 pitch。
在一个 2-landmark 2-pose 的最小 SLAM 例子中，计算两步的 $H_1$ 和 $H_2$，检查 $\text{null}(O)$（$O=[H_1; H_2 F]$）是否为 3 维。

§A2.5 First-Estimates Jacobian (FEJ) 与 Observability-Constrained EKF ⭐⭐⭐¶

FEJ：把线性化点钉死¶

Huang, Mourikis, Roumeliotis (ICRA 2008) 的修正方案极其简单：Jacobian 求值点从"当前估计 $\hat{x}_{k|k-1}$"改成"首次可用估计 $\hat{x}^\star$"。

\[ F_k^{\text{FEJ}} = \left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{k|k-1}^\star},\quad H_k^{\text{FEJ}} = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}_{k|k-1}^\star} \]

具体定义： - 机器人位姿：propagation 时用 propagation 前的 mean 一次（之后不再更换）； - 路标：线性化点永远是首次初始化时的位置。

这样保证**不同时刻的不可观测子空间沿同一个线性化流形对齐**，虚假信息增益的代数来源被消除。FEJ 不改变 EKF 算法结构，只改 Jacobian 求值点——工程改动不过几行代码，但需要 state 对象里多存一份 fej_value（这正是 OpenVINS ov_type::Type 基类里 _fej 字段的设计）。

如果 FEJ 只在部分 Jacobian 上生效会怎样？ 假设开发者在观测 Jacobian 上启用了 FEJ（路标位置用首次估计值），但在传播 Jacobian 上仍用当前估计值。此时不可观测子空间在传播步仍会被错误旋转——修复只做了一半。实际后果是：偏航过自信的速度减慢了（因为观测端的虚假信息被抑制），但长时间运行后仍会出现一致性退化。这就是为什么 OpenVINS 的 do_fej 开关必须全局生效——传播 Jacobian、观测 Jacobian、特征三角化、相机内外参标定的所有线性化点都必须切换到首次估计。

OC-EKF：投影到不可观测子空间的补¶

Huang, Mourikis, Roumeliotis (IJRR 2010, DOI 10.1177/0278364909353640) 提出更普适的 Observability-Constrained EKF：在每一步强制

\[ \Phi_{k+1,k}\,N_k = N_{k+1},\quad N_k\text{ = 理想不可观测子空间基矩阵} \]

即传播 Jacobian 必须把 $N_k$ 映到 $N_{k+1}$。具体通过最优扰动

\[ \hat{F}_k = \arg\min_{\bar F}\|\bar F - F_k\|_F^2 \quad \text{s.t. } \bar F\, N_k = N_{k+1} \]

求解（有闭式 projection 解）。OC-EKF 比 FEJ 多做一层"投影"，但 FEJ 本身在多数 VIO 场景已够用；OpenVINS 默认采用 FEJ（state_options.do_fej = true）。开源实现：github.com/rpng/ocekf-slam。

MSCKF 2.0 的一致性修正¶

Li & Mourikis (IJRR 2013, Vol.32 No.6 pp.690-711, DOI 10.1177/0278364913481251) 的核心贡献有三：(a) 证明标准 MSCKF 存在同样的偏航过自信；(b) 把 FEJ 应用到 IMU propagation Jacobian 与相机观测 Jacobian 的所有线性化点（官方 PDF: intra.ece.ucr.edu/~mourikis/papers/Li2013IJRR.pdf）；(c) 在线估计相机-IMU 外参 $^C_Iq,\,^Cp_I$ 与时间偏置 $t_d$。这篇是所有现代 VIO 滤波器的实现圣经，务必精读。

伏笔：为什么 IEKF 能从根本上解决¶

FEJ 是局部一致性修正：它只改变 Jacobian 求值点，不改变误差的代数结构。**Invariant EKF（Barrau-Bonnabel 2017，IEEE TAC 62(4)）**的做法是把状态直接定义在李群 $SE_2(3)$ 上，并选定与系统和观测相容的左/右不变误差。本文在 5-A3 采用 $\eta^L=X^{-1}\hat X$、$\eta^R=\hat X X^{-1}$；有些文献写 $\hat X^{-1}X$，那是互逆约定，线性化后会带来符号变化。选定约定后，误差动力学的 Jacobian 可精确地与状态轨迹无关——不可观测方向天然保持，一致性从代数层面自动满足。这是 5-A3 子专题的核心内容，本文不展开。

§A2.6 Unscented Transform：确定性采样传播 ⭐⭐¶

动机¶

EKF 通过线性化近似非线性——这是**对 $g$ 的近似**。UT 的哲学相反：不近似 $g$，而是用一组确定性的 sigma points 近似输入分布 $p(x)$，然后把这些 sigma points 逐个喂进真实的 $g$，再从输出点集统计均值协方差。口号："It is easier to approximate a probability distribution than to approximate an arbitrary nonlinear function"（Julier-Uhlmann 2004 原话）。

数学推导（对称 2n+1 点集）¶

给定 $x\sim\mathcal{N}(\mu,P)$，$x\in\mathbb{R}^n$，Scaled Unscented Transform（Julier ACC 2002 单作者，DOI 10.1109/ACC.2002.1025369；PDF: cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF）生成 $2n+1$ 个 sigma points。令 $\lambda = \alpha^2(n+\kappa)-n$，$\gamma = \sqrt{n+\lambda}$，$S=\text{chol}(P)$（$P=SS^\top$，$[S]_i$ 为第 $i$ 列）：

\[ \chi_0 = \mu,\quad \chi_i = \mu + \gamma\,[S]_i,\quad \chi_{n+i} = \mu - \gamma\,[S]_i,\quad i=1,\dots,n \]

权重：

\[ W_0^m = \frac{\lambda}{n+\lambda},\quad W_0^c = \frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta),\quad W_i^m = W_i^c = \frac{1}{2(n+\lambda)}\ (i\geq 1) \]

非线性传播 $\mathcal{Y}_i = g(\chi_i)$ 后：

\[ \mu_y \approx \sum_{i=0}^{2n} W_i^m\,\mathcal{Y}_i,\quad P_y\approx\sum_{i=0}^{2n}W_i^c(\mathcal{Y}_i-\mu_y)(\mathcal{Y}_i-\mu_y)^\top \]

交叉协方差 $P_{xy}\approx\sum W_i^c(\chi_i-\mu)(\mathcal{Y}_i-\mu_y)^\top$。

$\alpha$, $\beta$, $\kappa$ 的含义¶

$\alpha \in [10^{-4}, 1]$：控制 sigma points 的散布半径。$\alpha$ 小 → 点靠近 $\mu$，抑制高阶非线性误差，但丢失大范围曲率信息。
$\beta \ge 0$：先验分布形状修正。$\beta=2$ 对严格高斯最优（匹配 4 阶矩 kurtosis=3）；$\beta=0$ 为无修正。
$\kappa \in \mathbb{R}$：次级调节。常见选择 $\kappa=0$ 或 $\kappa=3-n$（后者使一维下四阶矩精确，但 $n\geq 3$ 时会让 $W_0^m<0$，需监控协方差正定）。

推荐默认（Van der Merwe PhD 2004 §3.4）：$\alpha=0.001$, $\beta=2$, $\kappa=0$，适用于大多数机器人状态估计。

Scaled UT 为什么必要¶

原始 Julier 1995 的 UT 公式没有 $\alpha$：sigma 点展开半径 $\gamma=\sqrt{n+\kappa}$ 在高维 $n$ 大时随 $\sqrt n$ 增长，sigma points 会飞到 $\mu$ 很远处，落入 $g$ 的非线性剧烈区，反而导致均值估计偏差。$\alpha$ 提供了"先缩放，再补偿"的机制：把 sigma points 拉近 $\mu$ 保持局部线性化有效性，然后通过 $W_0^c$ 的 $(1-\alpha^2+\beta)$ 项把协方差补偿回来。详见 Julier 2002 §III。

UT 精度阶——三阶证明要点¶

对 $g(\mu+\delta)$ 做 Taylor 展开到四阶，$x\sim\mathcal{N}(\mu,P)$：

\[ \mathbb{E}[g(x)] = g(\mu) + \tfrac{1}{2}\mathrm{tr}(\nabla^2g\cdot P) + \text{(3 阶奇项=0)} + \tfrac{1}{8}\mathbb{E}[\nabla^4g:\delta^{\otimes 4}] + \cdots \]

对称 sigma points 配对 $\chi_i, \chi_{n+i}$ 满足 $\chi_{n+i}=2\mu-\chi_i$，令 $h(\epsilon)=g(\mu+\gamma\epsilon s)$，则 $\tfrac{1}{2}(h(+1)+h(-1))$ 消去所有奇数阶 $\epsilon$ 项——均值估计精确到 3 阶 Taylor。协方差估计到 2 阶；$\beta=2$ 使 4 阶矩系数对齐高斯真值。

相比 EKF（均值、协方差均 1 阶），UT 在相同 $O(n^3)$ 复杂度下**免费多出两阶精度**，且无需 Jacobian——这就是工程师放弃 EKF 而转 UKF 的最大动机。

线性退化¶

若 $g(x)=Ax+b$，则 $\mathcal{Y}_i=A\chi_i+b$，$\mu_y=A\mu+b$，$P_y=APA^\top$。UT 与线性 KF 结果完全一致——这是所有 sigma-point 方法必须满足的 sanity check。

⚠️ 编程陷阱：生成 sigma points 时用协方差对角线而非 Cholesky 因子。 如果 $P$ 非对角（状态之间有相关性），仅用 $\sqrt{P_{ii}}$ 沿坐标轴生成 sigma points 会完全忽略交叉协方差，导致传播后的均值和协方差都是错误的。**必须**用 $S=\text{chol}(P)$ 的列向量作为展开方向，因为 $S$ 的列向量包含了状态之间的完整相关结构。

练习¶

对一维高斯 $x\sim\mathcal{N}(2,\,4)$，手算三个 sigma points $\chi_0,\chi_1,\chi_2$（取 $\alpha=1,\beta=2,\kappa=0$）和对应权重，验证 $\sum W_i^m\chi_i=2$，$\sum W_i^c(\chi_i-2)^2=4$。
对 $g(x)=x^2$，用三个 sigma points 计算 $\mu_y,P_y$，并与 EKF（$g'(2)\cdot\sigma$）的结果比较。
解释为什么 $\alpha\to 0$ 时 sigma points 趋近 $\mu$ 但 $W_0^c$ 趋近 $-\infty$——这对协方差正定性意味着什么？

§A2.7 UKF 完整推导与参数选择 ⭐⭐¶

预测步（Additive 形式）¶

生成 $2n+1$ 个 sigma points $\{\chi_i^{k-1|k-1}\}$ 基于 $(\hat{x}_{k-1|k-1},P_{k-1|k-1})$；
通过过程模型传播：$\chi_i^{k|k-1} = f(\chi_i^{k-1|k-1},u_{k-1})$；
预测均值：$\hat{x}_{k|k-1}=\sum_{i=0}^{2n}W_i^m\chi_i^{k|k-1}$；
预测协方差：$P_{k|k-1}=\sum W_i^c(\chi_i^{k|k-1}-\hat{x}_{k|k-1})(\cdot)^\top + Q_{k-1}$。

更新步¶

（通常重新生成 sigma points 以反映 $Q$ 的加入；也可复用）经观测函数 $\mathcal{Z}_i=h(\chi_i^{k|k-1})$；
预测观测 $\hat{y}_k=\sum W_i^m\mathcal{Z}_i$；
Innovation 协方差 $P_{yy}=\sum W_i^c(\mathcal{Z}_i-\hat{y}_k)(\cdot)^\top + R_k$；
交叉协方差 $P_{xy}=\sum W_i^c(\chi_i^{k|k-1}-\hat{x}_{k|k-1})(\mathcal{Z}_i-\hat{y}_k)^\top$；
Kalman 增益 $K_k=P_{xy}P_{yy}^{-1}$；
$\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k(y_k-\hat{y}_k)$，$P_{k|k}=P_{k|k-1}-K_kP_{yy}K_k^\top$。

Additive vs Augmented UKF¶

Additive（本节公式）假设 $x_k=f(x_{k-1})+w_{k-1}$、$y_k=h(x_k)+v_k$ 噪声加性。Sigma points 数 $2n+1$，$Q,R$ 直接加到协方差，复杂度 $O(n^3)$。工程上 99% 的机器人系统用这个版本。

Augmented：扩展状态 $x_a=[x;w;v]$，$P_a=\text{blkdiag}(P,Q,R)$，维度 $n_a=n_x+n_w+n_v$，sigma points $2n_a+1$；若 $n_w=n_v=n_x$ 则共 $6n+1$ 点。优点：非加性噪声（如 IMU 噪声通过非线性旋转矩阵进入）精确到二阶；缺点：$O(n_a^3)$，对高状态维数贵。来源：Van der Merwe PhD 2004 §3.4；Wan-Van der Merwe Kalman Filtering and Neural Networks (2001) Ch.7 Table 7.2/7.3。

UKF vs EKF 对比¶

维度	EKF	UKF
近似对象	非线性函数 $g$ 一阶 Taylor	输入分布 $p(x)$ 的 sigma 采样
Jacobian	必须	不需要
均值精度	1 阶	3 阶（对称对消）
协方差精度	1 阶	2 阶（$\beta=2$ 时 4 阶矩对齐）
主循环复杂度	$O(n^3)$	$O(n^3)$（Cholesky 主导）+ $2n+1$ 次 $f/h$ 调用
对强非线性鲁棒性	差（发散风险）	优
对大初始不确定性鲁棒性	差	优
对不连续函数	失效（Jacobian 不存在）	可直接评估，但通常不可靠；应显式建模离散模式
易用性	推 Jacobian 繁琐	只需 $f,h$ 即可
工业成熟度	★★★★★（30 年）	★★★★（20 年）

$\alpha$/$\beta$/$\kappa$ 速查表¶

场景	$\alpha$	$\beta$	$\kappa$	备注
姿态估计（3 维 sigma）	$10^{-3}$	2	0	跟随 Wan-Van der Merwe 默认
组合导航（IMU+GNSS，15 维）	$10^{-3}$ 到 $10^{-2}$	2	0	太小 $\alpha$ 会让 $W^c$ 过大致病态
EKF-SLAM 替代 UKF-SLAM	$10^{-2}$	2	$3-n$	高维下 $3-n$ 会让 $W_0^m<0$，加 Joseph form 保护
van der Pol / Lorenz 数值实验	1	2	0	$\alpha=1$ 退化为原始 Julier UT
简单 1-2 维非线性	1	0	2	传统 Julier 选择

§A2.8 Square-Root UKF 与数值稳定性 ⭐⭐⭐¶

Cholesky 失败的根源¶

UKF 每步需要 $S=\text{chol}(P)$。$P$ 理论上始终半正定，但因为：(a) 浮点累积误差；(b) 协方差更新 $P_{k|k-1}=\sum W_i^c(\cdot)(\cdot)^\top+Q$ 中 $W_0^c$ 可能**为负**（当 $\alpha<1$ 或 $\kappa<0$）；(c) Kalman 更新后减法 $P_{k|k}=P_{k|k-1}-K_kP_{yy}K_k^\top$ 可能破坏正定性——实际系统中 Eigen::LLT 经常在运行数分钟后抛异常。

工程救援手段¶

Joseph form 协方差更新（见 §A2.2）——对线性减号失败最有效；
**LDLᵀ / SVD 分解**替代 Cholesky，容忍半正定：Eigen::LDLT、Eigen::SelfAdjointEigenSolver；
对称化：每步 $P\leftarrow\tfrac{1}{2}(P+P^\top)$ 消除非对称数值噪声；
正定化 floor：$P\leftarrow P+\varepsilon I$ 强制加对角 jitter；
Square-Root UKF：从根本上传播 $S$ 而非 $P$。

Square-Root UKF（Van der Merwe & Wan, ICASSP 2001, DOI 10.1109/ICASSP.2001.940586）¶

核心：不再存 $P$，只存 $S=\text{chol}(P)$。sigma points 生成直接用 $S$，协方差更新通过 QR 分解 和 Cholesky rank-1 update/downdate：

\[ S_{k|k-1} = \text{qr}\left(\left[\sqrt{W_1^c}(\chi_1^{k|k-1}-\hat{x}_{k|k-1})\ \cdots\ \sqrt{W_{2n}^c}(\chi_{2n}^{k|k-1}-\hat{x}_{k|k-1})\ \sqrt{Q}\right]\right) \]

然后用 cholupdate 处理 $W_0^c$ 的中心点贡献（若 $W_0^c<0$ 做 downdate）。复杂度**通用状态估计 $O(L^3)$ 与原始 UKF 相同，但**参数估计时降为 $O(L^2)$。实现：FilterPy 有 SquareRootUKF（需自行组装）；ROS robot_localization 用的是标准 UKF，在源码里通过 Eigen::LLT + LDLT fallback 处理。

§A2.9 Cubature Kalman Filter (CKF) ⭐⭐⭐¶

动机¶

UKF 在高维下有两大烦恼：(a) $\alpha$ 选得不好则 sigma 点飞太远或中心点协方差权重 $W_0^c<0$；(b) 中心点权重 $W_0^c$ 随 $n$ 增大可能负得更深（注意：非中心点权重 $W_i = 1/[2(n+\lambda)]$ 始终为正，仅中心点权重可能为负，但已足以导致协方差估计丢失正定性）。Arasaratnam & Haykin (IEEE TAC 54(6), 2009, DOI 10.1109/TAC.2009.2019800) 从数值积分理论出发重新推导：取消中心 sigma 点，用 三阶球径（spherical-radial）求积规则 给出一个简洁、全正权重、数值更稳定的方案。官方 PDF: soma.mcmaster.ca/papers/ckf_2009.pdf。

三阶球径求积规则¶

目标积分 $I[f]=\int_{\mathbb{R}^n}f(x)\mathcal{N}(x;0,I)dx$，做变量替换 $x=r\cdot y$，$y^\top y=1$，$r\in[0,\infty)$：

\[ I[f] = \int_0^\infty\int_{U_n}f(r y)\,r^{n-1}e^{-r^2/2}\,d\sigma(y)\,dr \]

对球面部分用 3 阶对称积分规则 $\{\pm e_i\}_{i=1}^n$（2n 个点），对径向部分用 1 阶 Gauss-Laguerre（在 $r^2=n$ 处取值），合并得到：

\[ I[f] \approx \sum_{i=1}^{2n}\frac{1}{2n}\,f(\xi_i),\quad \xi_i=\sqrt{n}\,e_i,\ \xi_{n+i}=-\sqrt{n}\,e_i \]

对任意三阶以下多项式精确。

CKF 算法¶

对 $x\sim\mathcal{N}(\mu,P)$，令 $S=\text{chol}(P)$，cubature points $X_i=\mu+S\xi_i$。预测-更新套用 UKF 的结构，只是：只有 2n 个点，权重一律 1/(2n)。没有 $\alpha,\beta,\kappa$，没有负权重，没有需要调参的麻烦。

CKF = UKF($\alpha=1, \beta=0, \kappa=0$) 的证明¶

代入 Scaled UT 公式：$\lambda=\alpha^2(n+\kappa)-n=1\cdot(n+0)-n=0$，故

$\gamma=\sqrt{n+\lambda}=\sqrt{n}$（与 CKF 半径一致）
$W_0^m=\lambda/(n+\lambda)=0$（中心点权重消失）
$W_0^c=0+(1-1+0)=0$
$W_i^m=W_i^c=1/(2(n+\lambda))=1/(2n)$

与 CKF 的 2n 点、等权 1/(2n) 完全相同。结论：CKF 是 Scaled UT 在 $(\alpha,\beta,\kappa)=(1,0,0)$ 下的特例。

CKF vs UKF¶

特性	UKF (typical $\alpha=0.001, \beta=2, \kappa=0$)	CKF
点数	2n+1	2n
权重	中心点权重 $W_0^c$ 可能为负（非中心点权重始终正）	全为 1/(2n)，严格正
需要调参	$\alpha, \beta, \kappa$	无
精度阶	3（配 $\beta=2$ 得 4 阶矩对齐）	3
高维数值稳定性	较差（$W_0$ 随 n 负得深）	优（无中心点问题）
$\beta$ 自由度带来的灵活性	有（可调 kurtosis）	无

Arasaratnam 原文定理 1 证明了 CKF 的协方差更新保持对称正定；定理 2 给出稳定性条件。CKF 在航天器姿态估计、深空导航（NASA JPL 部分任务）和高维金融状态空间模型中比 UKF 更受欢迎。

本质洞察：CKF 与 UKF 的核心区别不是"哪个更准"——两者在三阶精度下等价。真正的区别是 CKF 用全正权重消除了 UKF 的"负权重协方差病态"风险，代价是放弃了 $\beta$ 参数提供的四阶矩调节能力。工程上的选择准则很简单：如果你不确定 $\alpha,\beta,\kappa$ 该怎么选、或者状态维度高于 10，直接用 CKF。

练习¶

代入 $(\alpha,\beta,\kappa)=(1,0,0)$ 到 Scaled UT 公式，验证 $W_0^m=W_0^c=0$，$W_i=1/(2n)$，$\gamma=\sqrt{n}$——这就是 CKF。
对 $n=20$，计算 UKF（$\alpha=10^{-3},\kappa=0$）的 $W_0^c$ 值，验证它是否为负。

CKF 衍生¶

Cubature Kalman Smoother（Arasaratnam-Haykin, 2011, IEEE TSP 59(8)）：RTS 形式。
5 阶 CKF（Jia et al., Automatica 2013）：用 5 阶球径规则，$2n^2+1$ 点。
CD-CKF（Arasaratnam, Haykin, Hurd 2010, IEEE TSP, DOI 10.1109/TSP.2010.2056923）：连续-离散混合 CKF。

实现：FilterPy 有 filterpy.kalman.CubatureKalmanFilter（github.com/rlabbe/filterpy/blob/master/filterpy/kalman/CubatureKalmanFilter.py）。

§A2.10 Gauss-Hermite Kalman Filter 与 Sparse-Grid ⭐⭐⭐⭐¶

GHKF（Ito & Xiong, IEEE TAC 45(5), 2000, DOI 10.1109/9.855552）¶

Gauss-Hermite 是标准高斯权 $w(x)=\mathcal{N}(x;0,1)$ 下的一维最优 Gauss 求积：m 点规则对 $\leq 2m-1$ 阶多项式精确。节点 $\xi_i$ 为（概率论约定）Hermite 多项式 $He_m(x)$ 的根；权重由节点附近多项式导数给出。

多维张量积——维度灾难¶

n 维张量积 GHKF 需要 $m^n$ 个点。$m=3, n=10$：$3^{10}=59\,049$；$m=3,n=15$：$\approx 1.4\times 10^7$。这是 GHKF 只能用于 $n \le 4$ 的根本原因——在航天器姿态（n=3 或 6）、化工过程（n=2-4）这类低维高精度场景最合适。

Sparse-Grid Quadrature（SGQ）¶

Smolyak (Dokl. Akad. Nauk SSSR 4, 1963) 构造：从张量积中挑选"精度预算最有效"的子集，点数从 $m^n$ 降到 $O(n^k\log^{n-1}n)$ 级。

Jia, Xin, Cheng (Automatica 48(2), 2012, pp.327-341, DOI 10.1016/j.automatica.2011.08.057) 系统地把 L 级 Smolyak sparse grid 用作 Gaussian filter 的求积子：精度可调，点数对维度是多项式。相关应用工作：Jia, Xin, Cheng (JGCD 34(2), 2011, DOI 10.2514/1.52016) 在航天器姿态估计上验证 Sparse Gauss-Hermite Quadrature Filter。

统一视角¶

所有 Gaussian filter 的差异只在求积规则：

滤波器	积分规则	点数	精度（高斯下）
EKF	一阶 Taylor（伪积分）	1（$\mu$ 本身）	1
二阶 EKF	二阶 Taylor	1+Hessian	2
UKF	Scaled UT（球径-径向）	2n+1	均值 3，协方差 2
CKF	3 阶球径 + 1 阶径向	2n	3
5 阶 CKF	5 阶球径	$2n^2+1$	5
GHKF(m)	m 阶 Gauss-Hermite 张量积	m^n	2m-1
Sparse-Grid GHKF(L)	Smolyak 稀疏网格	O(n^L)	约 2L-1

这是 Särkkä & Svensson 2ed Ch.8 General Gaussian Filtering 的核心观点：所有这些滤波器都是同一个 Gaussian filter 模板（§8.1-8.2），仅积分规则不同： - §8.3 GHI，§8.4 GHKF - §8.5 Spherical Cubature，§8.6 CKF - §8.7 UT，§8.8 UKF - §8.9 Higher-Order CKF/UKF

§A2.11 迭代变体：IEKF 与 ISPKF ⭐⭐⭐⭐¶

IEKF：作为 Gauss-Newton（Bell & Cathey, IEEE TAC 38(2), 1993, DOI 10.1109/9.250476）¶

标准 EKF 更新只做**一次**线性化 $H_k$（在 $\hat{x}_{k|k-1}$）。IEKF 把更新步改成迭代：令 $\hat{x}^{(0)}=\hat{x}_{k|k-1}$，对 $j=0,1,\dots$：

\[ H^{(j)} = \left.\frac{\partial h}{\partial x}\right|_{\hat{x}^{(j)}},\quad K^{(j)}=P_{k|k-1}H^{(j)\top}(H^{(j)}P_{k|k-1}H^{(j)\top}+R_k)^{-1} \]

\[ \hat{x}^{(j+1)} = \hat{x}_{k|k-1} + K^{(j)}\left[y_k-h(\hat{x}^{(j)})-H^{(j)}(\hat{x}_{k|k-1}-\hat{x}^{(j)})\right] \]

收敛到 $\hat{x}^{(\infty)}$ 后，$P_{k|k}=(I-K^{(\infty)}H^{(\infty)})P_{k|k-1}$。

Bell-Cathey 定理：这个迭代**等价于对负对数后验 $-\log p(x_k|y_k,\hat{x}_{k|k-1})$ 做 Gauss-Newton**。原文摘要："the iterated Kalman filter (IKF) update is an application of the Gauss-Newton method for approximating a maximum likelihood estimate."——这是 Barfoot 2ed §4.2.6 "IEKF Is a MAP Estimator"（p.120）正式书写的源头。含 Levenberg-Marquardt / line-search 加强的 IEKF 见 Särkkä 2ed §7.5。

为什么重要：IEKF 把 EKF 一次性的"单步 Gauss-Newton"变成"完整 Gauss-Newton"；而因子图 SLAM（GTSAM、Ceres）就是**对整个时间窗口的 Gauss-Newton/LM**。IEKF 是滤波和优化之间的桥梁。

ISPKF（Iterated Sigma-Point KF；Barfoot 2ed §4.2.10, p.137）¶

UKF 版的迭代更新。Barfoot §4.2.11 "ISPKF Seeks the Posterior Mean" 证明 ISPKF 的收敛点是**后验均值**（而 IEKF 是 MAP）——这是 ISPKF 值得存在的关键理论结果。早期版本来自 Sibley 的 USC 博士论文工作（Sibley-Matthies-Sukhatme 的 sliding window filter 系列）。

与 Bundle Adjustment 的桥¶

EKF = 单步 Gauss-Newton（一次线性化）
IEKF = 完整 Gauss-Newton（迭代线性化，MAP）
BA / Factor Graph = 跨所有时间步的 Gauss-Newton（全局 MAP）
滑窗 BA (VINS-Mono) = 窗口内 Gauss-Newton + 边缘化先验

这是 5-B（因子图）将详述的脉络。

§A2.12 实战：MSCKF 与 OpenVINS 的 EKF 工程 ⭐⭐¶

MSCKF 状态结构（Mourikis-Roumeliotis, ICRA 2007, DOI 10.1109/ROBOT.2007.364024；PDF: www-users.cse.umn.edu/~stergios/papers/ICRA07-MSCKF.pdf）¶

IMU 状态（存储 16 维，误差态 15 维，JPL 约定）：

\[ x_{\text{IMU}} = \begin{bmatrix}{}^I_G\bar{q}\\ b_g\\ {}^Gv_I\\ b_a\\ {}^Gp_I\end{bmatrix}\in \mathbb{R}^{16}\ \text{存储},\ \delta x_{\text{IMU}}\in\mathbb{R}^{15} \]

其中 ${}^I_G\bar{q}$ 是从全局到 IMU 系的 JPL 单位四元数（注意 JPL 与 Hamilton 约定反向，见下）；$\delta\theta\in\mathbb{R}^3$ 是小旋转误差。

相机克隆（每帧存储 7 维，误差 6 维）：

\[ x_{C_i} = \begin{bmatrix}{}^{C_i}_G\bar{q}\\ {}^Gp_{C_i}\end{bmatrix}\in\mathbb{R}^7 \]

总状态：$x=[x_{\text{IMU}}^\top, x_{C_1}^\top,\dots,x_{C_N}^\top]^\top$，$N$ 通常 10-20。

Null-space Projection（消元特征点）¶

每个特征 $f_j$ 被 $M$ 个相机观测到，线性化残差：

\[ r_j = H_x^j \delta x + H_f^j \delta f_j + n_j,\quad H_x^j\in\mathbb{R}^{2M\times\dim x},\ H_f^j\in\mathbb{R}^{2M\times 3} \]

构造 $H_f^j$ 的左零空间基 $A\in\mathbb{R}^{2M\times(2M-3)}$（$A^\top H_f^j=0$），左乘 $A^\top$：

\[ r_j' = A^\top H_x^j \delta x + A^\top n_j \]

特征维消失。把所有特征的 $r_j'$ 堆叠后再做 EKF 更新。复杂度：状态维 $O(N)$（相机数），更新 $O(N^2M)$（$M$=特征数），线性于特征数；对比 EKF-SLAM 的 $O(M^2)$。实现：ov_msckf::UpdaterHelper::nullspace_project_inplace（QR 实现 Givens / Householder）+ measurement_compress_inplace（QR 进一步压缩行数到状态维）。

OpenVINS 源码地图（github.com/rpng/open_vins，UDel RPNG Huang 组）¶

模块/类	文件	职责
`ov_msckf::VioManager`	`ov_msckf/src/core/VioManager.cpp`	主调度，实例化 Propagator/Updater
`ov_msckf::State`	`ov_msckf/src/state/State.h`	状态容器：`_imu`, `_clones_IMU`, `_features_SLAM`, `_calib_IMUtoCAM`, `_cam_intrinsics`, `_calib_dt_CAMtoIMU`
`ov_type::StateHelper`	`ov_msckf/src/state/StateHelper.h`	协方差 I/O：`EKFUpdate`, `marginalize`, `augment_clone`, `marginalize_slam`
`ov_msckf::Propagator`	`ov_msckf/src/state/Propagator.cpp`	IMU 连续-离散传播（RK4/梯形），`propagate_and_clone`
`ov_msckf::UpdaterMSCKF`	`ov_msckf/src/update/UpdaterMSCKF.cpp`	MSCKF 更新：三角化 → G-N 细化 → Jacobian → null-space 投影 → QR 压缩 → $\chi^2$ 门限 → `EKFUpdate`
`ov_msckf::UpdaterSLAM`	`ov_msckf/src/update/UpdaterSLAM.cpp`	混合模式：部分特征入状态做 SLAM 更新
`ov_msckf::UpdaterHelper`	`ov_msckf/src/update/UpdaterHelper.cpp`	Jacobian 计算：`get_feature_jacobian_full`, `get_feature_jacobian_representation`, `nullspace_project_inplace`, `measurement_compress_inplace`
`ov_msckf::UpdaterZeroVelocity`	`ov_msckf/src/update/UpdaterZeroVelocity.h`	零速度更新（提高静止精度）
`ov_type::Type` / `ov_type::IMU` / `ov_type::PoseJPL`	`ov_core/src/types/`	类型系统，含 `_id`（协方差中的偏移）、`_value`、`_fej`（线性化点）

FEJ 在 OpenVINS 中的开关¶

state->_options.do_fej = true 时，UpdaterHelper 所有 Jacobian 求值切换到 FEJ 值：

Eigen::Vector3d p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;
Eigen::Matrix3d R_GtoI = (state->_options.do_fej) ? state->_imu->Rot_fej() : state->_imu->Rot();
Eigen::Vector3d p_IinG = (state->_options.do_fej) ? state->_imu->pos_fej() : state->_imu->pos();

OpenVINS 默认 do_fej=true。关闭后可以用 EuRoC 数据集直接复现 Huang-Mourikis 2010 所描述的偏航过自信（NEES 迅速飞出 $\chi^2$ 上界）。

JPL vs Hamilton 四元数¶

JPL（MSCKF/OpenVINS 原生）：$ij=-k$，$jk=-i$，$ki=-j$；四元数 $\bar{q}=[q_x,q_y,q_z,q_w]^\top$ 存储顺序（scalar last），表示 $R(\bar{q})$ 为从右侧帧到左侧帧的旋转矩阵；乘法 $\bar{q}_1\otimes\bar{q}_2$ 对应旋转复合 $R_1R_2$。

Hamilton（VINS-Mono/Ceres/ROS tf/Eigen::Quaterniond）：$ij=k$；scalar first 或 last 约定各异（Eigen 内部是 scalar last 存储但构造函数 scalar first）。

邱笑晨《采用 Hamilton 四元数的低成本 IMU 误差方程详细推导》（github.com/PetWorm/IMU_error_state_propagation_doc）与 LARVIO（github.com/PetWorm/LARVIO）的主要贡献就是把 MSCKF 重写成 Hamilton 约定，便于与 ROS 生态对接。工程陷阱：在 MSCKF 代码里混用 Eigen::Quaterniond（Hamilton）而未显式转换，符号会错得非常隐蔽——这是 VIO 工程最常见的低级故障之一。

OpenVINS 论文与性能¶

Geneva, Eckenhoff, Lee, Yang, Huang, "OpenVINS: A Research Platform for Visual-Inertial Estimation," ICRA 2020, pp. 4666-4672, DOI 10.1109/ICRA40945.2020.9196524；PDF: yangyulin.net/papers/2020_icra_ov.pdf。特性：on-manifold sliding-window MSCKF；在线相机内外参与时延 $t_d$ 标定；多种 landmark 表示（GLOBAL_FULL_INVERSE_DEPTH、ANCHORED_FULL_INVERSE_DEPTH、ANCHORED_MSCKF_INVERSE_DEPTH 等）；FEJ 与 OC 选项；完整仿真器 ov_msckf::Simulator；2019 IROS FPV Drone Racing VIO Competition 冠军。

VINS-Mono 作为滤波-优化对照¶

Qin, Li, Shen (T-RO 34(4), 2018, DOI 10.1109/TRO.2018.2853729; arXiv:1708.03852) 的 VINS-Mono 是**滑窗优化**（Ceres），不是滤波——但在同一个 VIO 问题上提供了 MAP 参考解。两者对比教学意义： - OpenVINS/MSCKF = 单步 G-N 迭代（EKF） + FEJ 一致性修正 - VINS-Mono = 窗口内多步 G-N（Ceres） + 边缘化先验 - VINS-Mono 的 pose-graph（pose_graph/）只优化 4 维（$x,y,z,\psi$），因为 roll/pitch 被 IMU 重力充分约束 → 与 VIO 不可观测性完全吻合。

ROS robot_localization（Moore & Stouch, IAS-13, 2014）¶

仓库 github.com/cra-ros-pkg/robot_localization。15 维通用运动学 EKF：$(X,Y,Z,\text{roll},\text{pitch},\text{yaw},\dot X,\dot Y,\dot Z,\dot\phi,\dot\theta,\dot\psi,\ddot X,\ddot Y,\ddot Z)$。核心文件：include/robot_localization/filter_base.h（抽象 FilterBase）、include/robot_localization/ekf.h+src/ekf.cpp、include/robot_localization/ukf.h+src/ukf.cpp、include/robot_localization/ros_filter.h+src/ros_filter.cpp（模板 RosFilter<T> 订阅 nav_msgs/Odometry、sensor_msgs/Imu 等）、src/navsat_transform_node.cpp（GPS→UTM）。UKF 默认用 Merwe scaled sigma points。这是**所有 ROS 移动机器人里程计融合的工业事实标准**。

ROVIO（ETH ASL，对照）¶

Bloesch et al. "Robust Visual Inertial Odometry Using a Direct EKF-Based Approach," IROS 2015, DOI 10.1109/IROS.2015.7353389；期刊版 IJRR 36(10), 2017, DOI 10.1177/0278364917728574。仓库 github.com/ethz-asl/rovio（submodule lightweight_filtering，依赖 kindr）。特色：直接法 EKF，innovation 是光度误差（useDirectMethod=true）或重投影误差；特征参数化 $(\text{bearing vector} + \text{inverse distance})$；多层金字塔 patch。配置（cfg/rovio.info）：MahalanobisTh=9.21，UpdateNoise.pix=2，initCovFeature_0=0.5（distance）。

§A2.13 高斯和滤波器与粒子滤波：Kalman 族的边界 ⭐⭐⭐¶

Gaussian Sum Filter（GSF）¶

Kalman 族假设**单峰高斯后验**。多模态场景（数据关联不确定、多假设跟踪、障碍物轨迹分支）这个假设崩溃。GSF 用 $K$ 个高斯的加权混合：$p(x_k|y_{1:k})=\sum_{i=1}^K w_i^k\mathcal{N}(\mu_i^k,P_i^k)$，每个分量独立跑 EKF/UKF，权重按似然更新。问题：$K$ 随时间指数增长，需剪枝/合并（Mahalanobis 距离阈值）。Simon 2006 §13.8 给出完整算法。

Particle Filter（PF）¶

粒子滤波是 Kalman 族的**终极替代**，适用于：非高斯噪声、严重多模态、非线性强到线性化彻底失败的场景。步骤：(1) Sequential Importance Sampling（从 $q(x_k|x_{k-1},y_k)$ 采样 $N$ 个粒子）；(2) 按 $w_i\propto p(y_k|x_k^i)$ 更新权重；(3) 重采样（systematic/stratified resampling）避免粒子退化。陷阱：维数灾难——粒子数需指数增长 $O(e^n)$，实用上限 $n \le 6$。Thrun Ch.4；Barfoot 2ed §4.2.8（p.130）。

Rao-Blackwellized Particle Filter（RBPF / FastSLAM）¶

Montemerlo-Thrun-Koller-Wegbreit (AAAI 2002) "FastSLAM: A factored solution to the SLAM problem"。技巧：把状态分解为 $x=[x_p, x_l]$，$x_p$（机器人轨迹）用粒子采样，每个粒子内部对 $x_l$（地图）跑 EKF——即**每个粒子带 $M$ 个小 EKF**。这把维度从 $O(N_{\text{particles}}^{n_p+n_l})$ 降到 $O(N_{\text{particles}}\cdot n_l)$。FastSLAM 1.0/2.0 是 2002-2010 年代 2D 激光 SLAM 的主力，后被 GMapping / Cartographer 接棒。

§A2.14 汇总：非线性滤波族全景对比 ⭐⭐¶

滤波器	假设	精度（高斯下）	Jacobian	点数	典型复杂度	典型应用	代表库
EKF	单峰高斯，弱非线性	1 阶	需要	—	$O(n^3)$	古典组合导航、早期 SLAM	robot_localization `Ekf`, OpenVINS, Eigen 手写
二阶 EKF	单峰高斯	2 阶	需要 Jacobian+Hessian	—	$O(n^3)$+Hessian 代价	航天器姿态高精度场景	少见开源
IEKF（Iterated EKF）	单峰高斯 + 可 G-N 收敛	收敛到 MAP	每次迭代需要	—	$O(k \cdot n^3)$（k 迭代）	ROVIO（直接法）、光度 EKF	ROVIO, Barfoot slambook 练习
UKF	单峰高斯	均值 3，协方差 2	不需要	2n+1	$O(n^3)$	通用非线性估计	FilterPy `UnscentedKalmanFilter`, ros-ukf
Scaled UKF	单峰高斯	同上	不需要	2n+1	$O(n^3)$	高维（n>10）状态	FilterPy `MerweScaledSigmaPoints`
Square-Root UKF	单峰高斯	3/2	不需要	2n+1	$O(n^3)$（更稳）	长时间运行的嵌入式系统	自行实现；SR-UKF ICASSP 2001
CKF	单峰高斯	3	不需要	2n	$O(n^3)$	高维姿态/深空导航	FilterPy `CubatureKalmanFilter`
5 阶 CKF	单峰高斯	5	不需要	$2n^2+1$	$O(n^3)$+点数	高精度、中等维度	Jia 组代码（非官方）
GHKF(m)	单峰高斯	2m-1	不需要	m^n	$O(m^n \cdot n^2)$	航天器姿态（$n \le 4$）	MATLAB Sensor Fusion, EEA-sensors BFS 代码
Sparse-Grid GHKF(L)	单峰高斯	$\approx 2L-1$	不需要	O(n^L)	中维可用	航天器、过程控制	Jia 2012 工作
InEKF (Invariant EKF)	李群上；右/左不变误差	精确消除状态轨迹依赖	需要（但不随状态变）	—	$O(n^3)$	现代 VIO、腿式机器人	kalmanif `InvariantExtendedKalmanFilter`（→ 5-A3）

Sigma Point 家族对比¶

Sigma 集	点数	权重	精度阶	适用维度	来源
Julier Symmetric Set（原始 UT）	2n+1	含 $W_0=\kappa/(n+\kappa)$	均值 3	低-中	Julier-Uhlmann 1997
Merwe Scaled Set	2n+1	由 $\alpha,\beta,\kappa$ 参数化	均值 3，$\beta=2$ 时匹配 4 阶矩	低-高	Van der Merwe PhD 2004
Simplex Sigma Points	n+1	非对称	仅均值	嵌入式低算力	Julier 2003
Spherical-Radial Cubature	2n	全部 1/(2n)	3	中-高	Arasaratnam-Haykin 2009
5 阶 Cubature	$2n^2+1$	混合权	5	中维	Jia-Xin-Cheng 2013
Gauss-Hermite Tensor Product	m^n	Hermite 多项式根	2m-1 每维	仅 $n \le 4$	Ito-Xiong 2000
Smolyak Sparse Grid	O(n^L)	含负权	可调	中-高	Smolyak 1963 / Jia 2012

关键公式速查¶

步骤	EKF	UKF	CKF
预测均值	$f(\hat{x}_{k-1	k-1})$	$\sum W_i^m f(\chi_i)$
预测协方差	$F P F^\top + Q$	$\sum W_i^c(\cdot)(\cdot)^\top+Q$	$\frac{1}{2n}\sum(\cdot)(\cdot)^\top+Q$
预测观测	$h(\hat{x}_{k	k-1})$	$\sum W_i^m h(\chi_i)$
交叉协方差	$PH^\top$	$\sum W_i^c(\chi_i-\hat x)(h(\chi_i)-\hat y)^\top$	同 UKF 式（等权）
Kalman 增益	$K=PH^\top S^{-1}$	$K=P_{xy}P_{yy}^{-1}$	$K=P_{xy}P_{yy}^{-1}$
均值更新	$\hat{x}+K\tilde{y}$	$\hat{x}+K(y-\hat y)$	同左
协方差更新	Joseph form	$P-KP_{yy}K^\top$	$P-KP_{yy}K^\top$

必读论文表¶

论文	作者	年份	标识	贡献	档位
A New Approach for Filtering Nonlinear Systems	Julier, Uhlmann, Durrant-Whyte	1995	ACC, DOI 10.1109/ACC.1995.529783	UT 首次公开	3
Unscented Filtering and Nonlinear Estimation	Julier & Uhlmann	2004	Proc. IEEE 92(3):401-422, DOI 10.1109/JPROC.2003.823141	UT 综述与理论	3
The Scaled Unscented Transformation	Julier (单作者)	2002	ACC pp.4555-4559, DOI 10.1109/ACC.2002.1025369	$\alpha,\beta,\kappa$ 参数化	3
The Unscented Kalman Filter for Nonlinear Estimation	Wan & Van der Merwe	2000	AS-SPCC pp.153-158, DOI 10.1109/ASSPCC.2000.882463	UKF 完整算法	3
Sigma-Point Kalman Filters for Probabilistic Inference	Van der Merwe (PhD)	2004	OHSU, DOI 10.6083/M4Z60KZ5	SPKF 家族最权威综述	3-4
The Square-Root UKF	Van der Merwe & Wan	2001	ICASSP pp.3461-3464, DOI 10.1109/ICASSP.2001.940586	SR-UKF	4
Cubature Kalman Filters	Arasaratnam & Haykin	2009	IEEE TAC 54(6):1254-1269, DOI 10.1109/TAC.2009.2019800	CKF 开山	4
Gaussian Filters for Nonlinear Filtering Problems	Ito & Xiong	2000	IEEE TAC 45(5):910-927, DOI 10.1109/9.855552	Gaussian Filter 统一框架 + GHKF	4
Sparse-grid quadrature nonlinear filtering	Jia, Xin, Cheng	2012	Automatica 48(2):327-341, DOI 10.1016/j.automatica.2011.08.057	Sparse-grid Gaussian filter	4
Analysis and Improvement of the Consistency of EKF-Based SLAM	Huang, Mourikis, Roumeliotis	2008	ICRA pp.473-479, DOI 10.1109/ROBOT.2008.4543252	FEJ 起源	3-4
Observability-based Rules for Designing Consistent EKF SLAM Estimators	Huang, Mourikis, Roumeliotis	2010	IJRR 29(5):502-528, DOI 10.1177/0278364909353640	OC-EKF	4
A Multi-State Constraint KF for Vision-aided Inertial Navigation	Mourikis & Roumeliotis	2007	ICRA, DOI 10.1109/ROBOT.2007.364024	MSCKF 原始	3-4
High-precision, Consistent EKF-based VIO	Li & Mourikis	2013	IJRR 32(6):690-711, DOI 10.1177/0278364913481251	MSCKF 2.0 + FEJ	4
Consistency Analysis and Improvement of Vision-aided Inertial Navigation	Hesch, Kottas, Bowman, Roumeliotis	2014	IEEE T-RO 30(1):158-176, DOI 10.1109/TRO.2013.2277549	VIO 4 维不可观	4
The Iterated Kalman Filter Update as a Gauss-Newton Method	Bell & Cathey	1993	IEEE TAC 38(2):294-297, DOI 10.1109/9.250476	IEKF = G-N	3
OpenVINS: A Research Platform for VI Estimation	Geneva et al.	2020	ICRA pp.4666-4672, DOI 10.1109/ICRA40945.2020.9196524	OpenVINS 工程实现	3
VINS-Mono	Qin, Li, Shen	2018	T-RO 34(4):1004-1020, DOI 10.1109/TRO.2018.2853729	滑窗 VIO 对照	3
Robust VIO Using a Direct EKF-Based Approach (ROVIO)	Bloesch et al.	2015	IROS, DOI 10.1109/IROS.2015.7353389	直接法 EKF-VIO	3
A Generalized EKF Implementation for ROS	Moore & Stouch	2014	IAS-13	robot_localization 起源	3
Joseph Formulation of Unscented and Quadrature Filters	Zanetti & DeMars	2013	JGCD 36(6):1860-1864, DOI 10.2514/1.59935	Joseph form 推广到 UKF/CKF	4

C++/Python 库映射表¶

库	语言	滤波器类	关键文件/路径	备注
OpenVINS	C++	`ov_msckf::UpdaterMSCKF`	`ov_msckf/src/update/UpdaterMSCKF.cpp`	MSCKF 工程金标，FEJ 默认开
ROVIO	C++	direct EKF	`ethz-asl/rovio` + `lightweight_filtering` submodule	直接法 VIO
kalmanif	C++ header-only	`ExtendedKalmanFilter`, `SquareRootExtendedKalmanFilter`, `InvariantExtendedKalmanFilter`, `UnscentedKalmanFilterManifolds`	`artivis/kalmanif` include/	基于 manif，有 SE(2)/SE(3)/$SE_2(3)$ 示例
robot_localization	C++ (ROS/ROS2)	`RobotLocalization::Ekf`, `RobotLocalization::Ukf`	`include/robot_localization/ekf.h`, `ukf.h`	工业 ROS 标准
GTSAM	C++/Python	`gtsam::ExtendedKalmanFilter`	`gtsam/nonlinear/ExtendedKalmanFilter.h`	主打因子图；有 EKF 接口
FilterPy	Python	`KalmanFilter`, `ExtendedKalmanFilter`, `UnscentedKalmanFilter`, `CubatureKalmanFilter`, `SquareRootKalmanFilter`, `IMMEstimator`, `InformationFilter`, `EnsembleKalmanFilter`, `FadingKalmanFilter`, `MMAEFilterBank`, `FixedLagSmoother`	`filterpy/kalman/*.py`	教学 + 工业轻量级
FilterPy sigma 生成器	Python	`MerweScaledSigmaPoints`, `JulierSigmaPoints`, `SimplexSigmaPoints`	同上	配 UKF 使用
UKF-M	Python+MATLAB	流形 UKF	`CAOR-MINES-ParisTech/ukfm`	→ 5-A3
Eigen + 手写 EKF	C++	自行组装	参考 slambook2 或《自动驾驶中的 SLAM 技术》	理解用
Ceres Jet	C++	AutoDiff Jacobian	`ceres-solver/include/ceres/jet.h`	EKF Jacobian 来源
JAX / PyTorch	Python	`jax.jacfwd/jacrev`, `torch.func.jacfwd`	—	Python EKF AutoDiff
MATLAB	MATLAB	`extendedKalmanFilter`, `unscentedKalmanFilter`	Sensor Fusion Toolbox	原型快

常见陷阱表（15 条）¶

#	陷阱	后果	对策
1	Jacobian 在零姿态/奇异构型处退化	协方差爆炸或奇异	切换最小表示（四元数→旋转向量）、用流形扰动
2	UKF 协方差 Cholesky 分解失败	`Eigen::LLT` 抛异常	LDLᵀ fallback、对称化、SR-UKF
3	$\alpha$ 选得太大	sigma points 飞出 $g$ 线性区	$\alpha$ 从 $10^{-3}$ 起调
4	$\beta=0$ 在高斯场景	损失 4 阶矩精度	高斯先验永远用 $\beta=2$
5	FEJ 没全局生效	只部分抑制偏航过自信	所有 Jacobian 求值点都要切 FEJ
6	MSCKF 克隆窗口太大	状态维爆炸 $O(N^3)$	滑窗保持 $N \le 20$，边缘化旧克隆
7	RTK-GNSS 观测噪声设得太小	EKF 被带偏到 GPS 跳变	$\chi^2$ 门限拒绝异常观测；robust loss
8	数值微分步长选错	Jacobian 被舍入误差淹没	中心差分 $h\approx 6\times 10^{-6}$
9	Jacobian 符号反向	滤波器慢慢发散	用 AutoDiff 对拍验证
10	未用 Joseph form	协方差失去正定	所有生产 EKF 强制 Joseph form
11	JPL vs Hamilton 四元数混用	姿态镜像、隐蔽 bug	整个项目统一一种约定，边界显式转换
12	观测时间戳与状态时间戳不对齐	滤波器吸收过时信息	OOSM handling；缓冲延迟观测
13	IMU bias 没随状态协方差增广	bias 漂移不被建模	bias 必须进状态
14	粒子数设得过少（PF）	粒子退化	systematic resampling；$N_{\text{eff}}$ 监控
15	协方差初始化过自信	滤波器锁死在错初值	$P_0$ 宁大勿小；MSCKF 中 `set_initial_covariance` 调大

§A2.15 学习资源汇总¶

免费核心资源¶

Barfoot PDF: asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf （2024 2ed）
Särkkä & Svensson PDF: users.aalto.fi/~ssarkka/pub/bfs_book_2023_online.pdf （2023 2ed）
Särkkä 2013 1ed PDF: users.aalto.fi/~ssarkka/pub/cup_book_online_20131111.pdf
Thrun Probabilistic Robotics PDF: docs.ufpr.br/~danielsantos/ProbabilisticRobotics.pdf
Van der Merwe PhD OHSU Digital Collections: digitalcollections.ohsu.edu/record/8
Arasaratnam CKF PDF: soma.mcmaster.ca/papers/ckf_2009.pdf
Li-Mourikis MSCKF 2.0 PDF: intra.ece.ucr.edu/~mourikis/papers/Li2013IJRR.pdf
Huang-Mourikis 2010 IJRR: www-users.cse.umn.edu/~stergios/papers/IJRR-Observability-based-rules-consistency-2010.pdf
Huang-Mourikis 2008 ICRA FEJ: www-users.cse.umn.edu/~stergios/papers/icra08-fej.pdf
Mourikis-Roumeliotis 2007 MSCKF: www-users.cse.umn.edu/~stergios/papers/ICRA07-MSCKF.pdf
Julier 2002 Scaled UT: cs.unc.edu/~welch/kalman/media/pdf/ACC02-IEEE1357.PDF
Julier-Uhlmann 2004 Proc IEEE: cs.ubc.ca/~murphyk/Papers/Julier_Uhlmann_mar04.pdf（注意有 2004 年 12 月勘误）
EEA-sensors BFS 代码: github.com/EEA-sensors/Bayesian-Filtering-and-Smoothing

视频资源¶

Cyrill Stachniss (Uni-Bonn):
"Mobile Sensing and Robotics" MSR1/MSR2 course（YouTube 完整播放列表）
"SLAM Course" 2013 Freiburg 播放列表（含 EKF-SLAM、FastSLAM）
KF/EKF 5 分钟系列: ipb.uni-bonn.de/5min/
Online training hub: ipb.uni-bonn.de/online-training-robotics/
Simo Särkkä (Aalto):
EUSIPCO 2014 Tutorial "Bayesian Filtering and Smoothing"（PDF: eurasip.org/Seminars/Tutorials/EUSIPCO2014%20Tutorial%20Bayesian%20Filtering%20and%20Smoothing.pdf）
Aalto 研究生课 "ELEC-E8105 Non-linear filtering and parameter estimation"
深蓝学院《从零手写 VIO》（贺一家/崔华坤/赵松/高翔主讲；配套代码 github.com/HeYijia/VINS-Course）
Roger Labbe Jupyter 电子书: rlabbe.github.io/Kalman-and-Bayesian-Filters-in-Python/ （配 FilterPy 的最佳学习路径）

中文社区优质资源¶

高翔《视觉 SLAM 十四讲》2ed: github.com/gaoxiang12/slambook2（后端优化 BA 为主，EKF/IEKF 仅理论讨论；无独立 EKF 示例）
高翔《自动驾驶中的 SLAM 技术》(2023): github.com/gaoxiang12/slam_in_autonomous_driving（Ch.3 ESKF 融合 IMU+GNSS，Ch.10 EKF 融合定位——这是最好的中文 EKF 代码教程）
崔华坤《VIO-Doc》: github.com/StevenCui/VIO-Doc（含《VINS 论文推导及代码解析》）
邱笑晨《预积分总结与公式推导》: github.com/PetWorm/IMU-Preintegration-Propogation-Doc
邱笑晨《采用 Hamilton 四元数的低成本 IMU 误差方程详细推导》: github.com/PetWorm/IMU_error_state_propagation_doc（对理解 MSCKF Hamilton 化非常关键）
邱笑晨 LARVIO（Hamilton-MSCKF 实现）: github.com/PetWorm/LARVIO
赵宇 (aipiano) 博客: aipiano.github.io/
《不变扩展卡尔曼滤波（一）/（二）/（三）》系列
VINS-Mono 中文注释版: github.com/ManiiXu/VINS-Mono-Learning, github.com/castiel520/VINS-Mono
VINS-Course（深蓝）: github.com/HeYijia/VINS-Course（纯 Eigen 后端，不依赖 ROS/Ceres/g2o，演示 LM/滑窗/鲁棒核）
vio_data_simulation: github.com/HeYijia/vio_data_simulation
知乎 VINS-Mono 代码解读：余世杰（五行缺帅 wangshuailpp）、崔华坤、童哲航、Rain-XIA、极品巧克力（CSDN）
泡泡机器人 SLAM（Stachniss 课程中文字幕版，B 站可搜）

§A2.16 学习时间预算¶

章节	档位 3（博士入学基线）	档位 4（进阶）
§A2.1–A2.3 EKF 基础与 Jacobian	8-10h	—
§A2.4–A2.5 一致性 + FEJ + OC-EKF	6-8h	严格证明 Huang 2010 IJRR 定理 +6-8h
§A2.6–A2.8 UT + UKF + SR-UKF	8-10h	SR-UKF 实现 +2-3h
§A2.9–A2.10 CKF + GHKF + Sparse-Grid	—	8-10h（必读 Arasaratnam 全文）
§A2.11 IEKF / ISPKF	—	4-6h（Bell-Cathey + Barfoot §4.2.6, §4.2.11）
§A2.12 MSCKF + OpenVINS 源码	6-8h	OC-MSCKF 源码 +3-4h
§A2.13 GSF + PF + RBPF	2-3h	+2-3h（Doucet PF 综述）
合计	30-40h	再加 20-25h

§A2.17 自测题（10 道）¶

档位 3

从贝叶斯递推出发，推导 EKF 的预测协方差 $P_{k|k-1}=FPF^\top+Q$——明确哪一步做了一阶 Taylor 近似，哪一步做了独立高斯假设。
证明线性系统（$g(x)=Ax+b$）下 UT 的输出均值与协方差与线性 KF 一致。
实现 Jacobian 的解析、中心差分、Ceres Jet 三种方式，对同一非线性函数（如 $f(x)=\sin(x_1)x_2+x_1^2$）对拍精度；讨论最优差分步长。
EKF-SLAM 在 2D 中的真实不可观测子空间是什么？标准 EKF 线性化后丢失了哪一维？给出 FEJ 的修正公式。
在 FilterPy 或自写代码上用 EKF 跟踪 Van der Pol 振子（$\dot x_1=x_2,\,\dot x_2=\mu(1-x_1^2)x_2-x_1$，$\mu=2$），观察何时发散；换 UKF 对比。

档位 3+

推导 UT 三阶精度：在 $x\sim\mathcal{N}(\mu,P)$ 下 $\mathbb{E}[g(x)]$ 的 Taylor 展开到 4 阶，证明对称 sigma 配对消去 3 阶奇项。
解释为什么 EKF-SLAM 的偏航过自信被 FEJ 修正后仍非一致（信息增益不为零），为什么 IEKF 能从代数层面彻底消除。

档位 4

证明 CKF 是 Scaled UT 在 $(\alpha,\beta,\kappa)=(1,0,0)$ 下的特例，验证两者在 $n=5$ 的具体点集与权重完全一致。
推导一维 Gauss-Hermite 求积的精度阶为 $2m-1$（用正交多项式理论）；解释多维张量积为何是 $m^n$ 点且精度不再提升。
在 OpenVINS 上复现 Huang 2010 IJRR 的 NEES 实验：用 EuRoC MH_05 数据集，开关 do_fej。单次 15 维 NEES 可对比 $\chi^2_{15,0.95}$ 单侧上界；若做 $N$ 次 Monte Carlo 平均，则对比 $\chi^2_{15N,0.95}/N$。

§A2.18 与其他子任务的桥梁¶

← 5-A1（KF 框架）：预测-更新双步结构、Joseph form、信息滤波对偶、NEES/NIS 一致性诊断——全部在 EKF/UKF 中直接复用。
→ 5-A3（流形滤波）：EKF → ESKF（error-state）→ IEKF（invariant）→ UKF-M 的李群化路径；Barrau-Bonnabel 的 IEKF 从根本上解决 FEJ/OC-EKF 的一致性病理。
→ 5-A4（迭代变体 + 全景对比）：ISPKF、迭代 EKF-LM、Rauch-Tung-Striebel smoother、MHE；本文的全景表格将被扩展到包含流形滤波。
→ 5-B（因子图）：EKF 更新 = 单步 Gauss-Newton；IEKF = 完整 Gauss-Newton（Bell-Cathey 1993）；BA = 时间窗口 Gauss-Newton。GTSAM 的 ExtendedKalmanFilter 即是因子图框架下的 EKF 特例，ISAM2 则可视为带边缘化的动态 IEKF。
→ 5-E（Certifiable SLAM）：EKF/MSCKF 给出**局部最优**估计但无全局最优证书；certifiable 方法（Rosen et al. SE-Sync、Yang et al. TEASER）通过半定松弛给出全局最优证明。EKF 族对应"快速可运行但可能陷入局部最优"，certifiable 对应"慢但可证明全局"。

总结：一个原则¶

EKF 用线性化近似非线性，UKF/CKF/GHKF 用确定性采样近似非线性——但它们都假设后验仍是高斯。

这一句话框定了整个经典非线性滤波族的能力边界。EKF 通过 Jacobian 走**函数侧**的近似，丢 2 阶以上项；UKF/CKF/GHKF 通过 sigma/cubature 点走**分布侧**的近似，点越多越准但维度灾难。不论哪条路，它们都共享一个**致命假设**：后验是单峰高斯。一旦遇到多模态（数据关联模糊）、强非线性（翻滚、碰撞、大初始不确定性）、不可观测方向（EKF-SLAM 全局偏航），高斯假设就会以不同方式失效——这就是 FEJ/OC-EKF 的一致性修正、IEKF 的李群化、Gaussian Sum 的混合模型、Particle Filter 的放弃高斯，以及 certifiable SLAM 跳出滤波框架的全部动机。

欧氏空间的 EKF 是错的——但它是一切流形滤波与因子图优化的出发点。把 Huang-Mourikis 2010、Li-Mourikis 2013、Bell-Cathey 1993 三篇背进肌肉记忆，你就能读懂过去二十年机器人估计理论的整条主线；再往前一步，就进入 5-A3 的不变滤波和 5-B 的因子图世界了。¶

§A2.19 非线性 Gaussian filter 的统一模板 ⭐⭐⭐¶

这一节解决什么问题：把 EKF、UKF、CKF、GHKF 放回同一个贝叶斯积分模板中，避免把它们记成互不相干的算法清单。

回顾 A1：线性高斯滤波之所以闭式成立，是因为两件事同时成立。

第一，高斯经过线性变换仍然是高斯。

第二，两个高斯相乘仍然是高斯。

非线性系统破坏第一件事。

设：

\[ x_k=f(x_{k-1})+w,\qquad w\sim\mathcal{N}(0,Q) \]

上一时刻后验为：

\[ x_{k-1}\sim\mathcal{N}(\mu,P) \]

预测分布是：

\[ p(x_k)=\int \mathcal{N}(x_k;f(x),Q)\mathcal{N}(x;\mu,P)\,dx \]

当 $f$ 非线性时，这个积分一般不是高斯。

Gaussian filter 的共同策略是：

仍然只保留均值和协方差。
用某种近似计算 $f(x)$ 的均值。
用某种近似计算 $f(x)$ 的协方差。
把结果重新投影成一个高斯。

因此核心积分变成：

\[ \mu_y=\mathbb{E}[g(x)] \]

\[ P_y=\mathbb{E}[(g(x)-\mu_y)(g(x)-\mu_y)^\top] \]

其中：

\[ x\sim\mathcal{N}(\mu,P) \]

所有经典非线性滤波器都在近似这两个积分。

方法	近似对象	近似方式	代价
EKF	函数 $g$	一阶 Taylor	需要 Jacobian
二阶 EKF	函数 $g$	二阶 Taylor	需要 Hessian
UKF	分布的矩	2n+1 个 sigma points	需要调 $\alpha$/$\beta$/$\kappa$
CKF	高斯积分	2n 个球径点	无调参
GHKF	高斯积分	Hermite 张量积	点数 $m^n$
Sparse-grid	高斯积分	Smolyak 稀疏网格	实现更复杂

本质洞察：EKF 近似的是“非线性函数”，UKF/CKF/GHKF 近似的是“非线性函数作用下的概率积分”。二者不是谁完全替代谁，而是把误差放在不同位置。

这个统一模板能解释很多工程现象。

如果 Jacobian 很容易写、状态维度很高、非线性较弱，EKF 往往最好。

如果 Jacobian 难写、状态维度中等、初值不确定性较大，UKF/CKF 更省开发成本。

如果状态维度很低但需要高精度积分，GHKF 值得考虑。

如果后验明显多峰，上述方法都不合适，应转向 Gaussian Sum Filter 或 Particle Filter。

非线性近似的三个误差来源¶

第一类是**矩误差**。

近似积分得到的均值和协方差不同于真实值。

EKF 丢掉 Hessian 项。

UKF/CKF 丢掉更高阶矩。

GHKF 点数不足时丢掉高阶多项式项。

第二类是**高斯投影误差**。

即使均值和协方差算得很准，真实分布也可能不是高斯。

例如 $y=x^2$，$x\sim\mathcal{N}(0,1)$。

$y$ 的分布只在非负半轴上，但高斯近似会给负数分配概率。

第三类是**递推累积误差**。

一次近似误差可能很小。

但滤波器每一步都把近似后验作为下一步先验。

小误差会被时间传播、量测更新和错误增益不断放大。

这也是为什么一致性诊断必须贯穿整个序列，而不是只看单帧误差。

练习¶

令 $x\sim\mathcal{N}(0,\sigma^2)$，$g(x)=x^2$，手算真实 $\mathbb{E}[g(x)]$，并比较 EKF 在 $\mu=0$ 处的预测均值。
对 $g(x)=\sin x$，说明当 $\sigma$ 从 0.01 增大到 1.0 时，EKF 为什么越来越差。
画出一个双峰后验，说明为什么任何单高斯滤波器都无法同时表达两个假设。

§A2.20 EKF 误差传播的逐步推导 ⭐⭐⭐⭐¶

这一节解决什么问题：从真实状态、估计状态和误差定义出发，推导 EKF 的误差动力学与协方差传播。

非线性动力学：

\[ x_k=f(x_{k-1},u_{k-1})+w_{k-1} \]

预测均值：

\[ \hat x^-_k=f(\hat x^+_{k-1},u_{k-1}) \]

定义上一时刻误差：

\[ \delta x^+_{k-1}=x_{k-1}-\hat x^+_{k-1} \]

把真实状态写成：

\[ x_{k-1}=\hat x^+_{k-1}+\delta x^+_{k-1} \]

代入真实动力学：

\[ x_k=f(\hat x^+_{k-1}+\delta x^+_{k-1},u_{k-1})+w_{k-1} \]

在 $\hat x^+_{k-1}$ 处一阶展开：

\[ f(\hat x^+_{k-1}+\delta x,u) \approx f(\hat x^+_{k-1},u) +F_{k-1}\delta x \]

其中：

\[ F_{k-1} = \left.\frac{\partial f}{\partial x}\right|_{\hat x^+_{k-1},u_{k-1}} \]

所以：

\[ x_k\approx \hat x^-_k+F_{k-1}\delta x^+_{k-1}+w_{k-1} \]

预测误差为：

\[ \delta x^-_k=x_k-\hat x^-_k \]

因此：

\[ \delta x^-_k\approx F_{k-1}\delta x^+_{k-1}+w_{k-1} \]

协方差传播：

\[ P^-_k = \mathbb{E}[\delta x^-_k\delta x_k^{-\top}] \approx F_{k-1}P^+_{k-1}F_{k-1}^\top+Q_{k-1} \]

这就是 EKF 预测。

注意此处的近似发生在误差动力学。

均值传播用的是原始非线性函数 $f$。

协方差传播用的是线性化后的误差模型。

观测更新同理。

观测模型：

\[ y_k=h(x_k)+v_k \]

预测观测：

\[ \hat y_k=h(\hat x^-_k) \]

把真实状态写成：

\[ x_k=\hat x^-_k+\delta x^-_k \]

在 $\hat x^-_k$ 处展开：

\[ h(\hat x^-_k+\delta x^-_k) \approx h(\hat x^-_k)+H_k\delta x^-_k \]

其中：

\[ H_k=\left.\frac{\partial h}{\partial x}\right|_{\hat x^-_k} \]

新息：

\[ r_k=y_k-h(\hat x^-_k) \]

代入：

\[ r_k\approx H_k\delta x^-_k+v_k \]

这把非线性观测更新变成 A1 的线性量测模型。

于是：

\[ S_k=H_kP^-_kH_k^\top+R_k \]

\[ K_k=P^-_kH_k^\top S_k^{-1} \]

\[ \hat x^+_k=\hat x^-_k+K_kr_k \]

\[ P^+_k=(I-K_kH_k)P^-_k(I-K_kH_k)^\top+K_kR_kK_k^\top \]

二阶残差从哪里来¶

Taylor 展开真正写到二阶是：

\[ f(\hat x+\delta x) = f(\hat x)+F\delta x+\frac12 \begin{bmatrix} \delta x^\top H_1\delta x\\ \cdots\\ \delta x^\top H_n\delta x \end{bmatrix} +O(\|\delta x\|^3) \]

EKF 丢掉二阶项。

取期望时，二阶项的均值不是零：

\[ \mathbb{E}[\delta x^\top H_i\delta x]=\mathrm{tr}(H_iP) \]

所以 EKF 均值偏差约为：

\[ b_i\approx\frac12\mathrm{tr}(H_iP) \]

这解释了两个现象。

当 $P$ 很小，二阶偏差小，EKF 表现好。

当曲率 $H_i$ 很大，即使 $P$ 不大，EKF 也可能偏。

EKF 的误差传播边界¶

条件	EKF 表现	原因
初值准确、噪声小、函数平滑	通常稳定	一阶近似覆盖真实误差
初值很差	容易发散	线性化点远离真实状态
曲率很强	均值系统性偏	Hessian 项不可忽略
量测频繁且强约束	可能稳定也可能过自信	更新快但虚假信息也累积快
不可观测方向存在	容易不一致	线性化子空间随估计漂移

练习¶

对 $f(x)=x^2$ 在 $\hat x=1$ 处写出 EKF 一阶传播，并计算真实二阶偏差。
对二维极坐标量测 $h(p_x,p_y)=\mathrm{atan2}(p_y,p_x)$，手推 Jacobian，并说明在距离原点很近时为什么危险。
说明为什么 EKF 的均值传播不是 $F\hat x$，而是 $f(\hat x)$。

§A2.21 Unscented Transform 的矩匹配来龙去脉 ⭐⭐⭐⭐¶

这一节解决什么问题：从一维对称采样开始，推导为什么 UT 要用成对 sigma points，以及权重如何匹配均值和协方差。

先看一维。

设：

\[ x\sim\mathcal{N}(\mu,\sigma^2) \]

想用三个点表示这个分布：

\[ \chi_0=\mu \]

\[ \chi_1=\mu+c\sigma \]

\[ \chi_2=\mu-c\sigma \]

权重为：

\[ W_0,\quad W_1=W_2=w \]

匹配总权重：

\[ W_0+2w=1 \]

匹配均值：

\[ W_0\mu+w(\mu+c\sigma)+w(\mu-c\sigma)=\mu \]

由于正负点对称，均值自动满足。

匹配方差：

\[ w(c\sigma)^2+w(-c\sigma)^2=2wc^2\sigma^2=\sigma^2 \]

所以：

\[ 2wc^2=1 \]

这说明 $c$ 和 $w$ 不是随意的。

点越远，权重越小。

点越近，权重越大。

高维中把 $\sigma$ 换成 Cholesky 因子 $S$。

沿每个主轴取正负点：

\[ \chi_i=\mu+\gamma S_i \]

\[ \chi_{n+i}=\mu-\gamma S_i \]

对称性保证奇数阶项消去。

尺度 $\gamma$ 控制点离均值多远。

Scaled UT 用：

\[ \gamma=\sqrt{n+\lambda} \]

\[ \lambda=\alpha^2(n+\kappa)-n \]

这相当于把“点距”和“中心权重”交给 $\alpha,\kappa$ 调节。

$\beta$ 不改变点位置。

$\beta$ 改变中心点协方差权重，用来补偿高斯分布的四阶峰度。

为什么 UT 能超过 EKF¶

对 $g(\mu+\delta)$ 展开：

\[ g(\mu+\delta) = g(\mu) +g'(\mu)\delta +\frac12g''(\mu)\delta^2 +\frac16g'''(\mu)\delta^3 +\cdots \]

高斯分布的三阶中心矩为 0。

对称 sigma points 的三阶加权和也为 0。

二阶矩通过构造被精确匹配。

因此 UT 在均值上保留了 $g''$ 对协方差的影响。

EKF 则直接取 $g(\mu)$，丢掉这一项。

这就是 UT 对大初始不确定性更稳的根本原因。

UT 的边界¶

UT 仍然只保留一个高斯。

当函数把单峰分布折叠成多峰分布时，UT 无法表达多峰。

例如角度归一化会把 $-\pi$ 和 $\pi$ 附近的点折叠到一起。

如果不在流形上正确处理角度均值，sigma points 的平均会落在错误方向。

这正是 A3 中 UKF-M 必须使用 $\boxplus,\boxminus$ 的原因。

练习¶

对一维 UT，令 $W_0=0$，求 $c$ 和 $w$，说明它如何对应 CKF 的一维特例。
对 $g(x)=x^2$，比较 EKF 与三点 UT 的预测均值。
解释为什么 sigma points 必须通过 Cholesky 因子生成，而不是只用协方差对角线。

§A2.21b UKF 在四元数/流形上的扩展预览：$\boxplus/\boxminus$ 算子 ⭐⭐⭐¶

这一节解决什么问题：§A2.21 结尾指出了 UKF 直接应用于角度/四元数时的根本困难——sigma points 的加权平均在非欧氏空间上没有几何意义。本节简要说明如何通过 $\boxplus/\boxminus$ 算子把 sigma points 的生成和回收搬到切空间，为 A3 的完整流形滤波做铺垫。

问题的根源¶

标准 UKF 在两个地方做"向量加法"：

生成 sigma points：$\chi_i = \mu + \gamma\,[S]_i$
回收统计量：$\mu_y = \sum W_i^m\,\mathcal{Y}_i$，$P_y = \sum W_i^c\,(\mathcal{Y}_i - \mu_y)(\cdot)^\top$

当 $\mu$ 是普通向量（位置、速度、bias）时，加法是自然的。但当 $\mu$ 包含**四元数**（或旋转矩阵、$SE(3)$ 元素）时：

$q + \delta$ 不再是单位四元数——它脱离了约束流形 $S^3$
$\frac{1}{N}\sum q_i$ 作为"均值"在几何上没有意义（想象把 $S^3$ 上的点线性平均，结果落在球内部）
协方差的差 $\mathcal{Y}_i - \mu_y$ 不属于切空间，其外积不是切空间上的二阶张量

类比：把地球上的城市坐标做加法平均，结果可能掉进地壳里。经纬度的"均值"必须在大圆上计算，不能简单做分量平均。

$\boxplus$ 和 $\boxminus$ 算子¶

Hertzberg, Wagner, Frese, Schröder (2013) 的 $\boxplus/\boxminus$ 框架为此提供了统一接口：

\[ \boxplus:\mathcal{M}\times\mathbb{R}^d\to\mathcal{M},\qquad x\boxplus\delta = x\circ\exp(\delta) \]

\[ \boxminus:\mathcal{M}\times\mathcal{M}\to\mathbb{R}^d,\qquad y\boxminus x = \log(x^{-1}\circ y) \]

其中 $\mathcal{M}$ 是流形（$SO(3)$、$S^3$、$SE(3)$ 等），$d$ 是切空间维度，$\exp/\log$ 是流形的指数/对数映射。

对四元数 $q\in S^3$（表示旋转）：

\[ q\boxplus\delta\theta = q\otimes\exp_q(\delta\theta),\qquad \delta\theta\in\mathbb{R}^3 \]

\[ q_2\boxminus q_1 = \log_q(q_1^{-1}\otimes q_2)\in\mathbb{R}^3 \]

UKF with $\boxplus/\boxminus$：sigma points 在切空间生成¶

修改后的 UKF 三步：

Step 1：生成 sigma points。协方差 $P$ 定义在**切空间** $\mathbb{R}^d$ 上（而非四元数的 $\mathbb{R}^4$）。Cholesky $P = SS^\top$，sigma points 为：

\[ \chi_0 = \hat x, \quad \chi_i = \hat x\boxplus(\gamma\,[S]_i),\quad \chi_{n+i} = \hat x\boxplus(-\gamma\,[S]_i) \]

每个 sigma point 都在流形上（是合法四元数/旋转矩阵）。

Step 2：传播。每个 sigma point 通过非线性函数 $\mathcal{Y}_i = f(\chi_i)$。因为 $\chi_i\in\mathcal{M}$，传播在流形上进行，无需归一化。

Step 3：回收均值和协方差。均值通过**流形上的迭代平均**计算：

\[ \bar y = \arg\min_{m\in\mathcal{M}}\sum_i W_i^m\,\|\mathcal{Y}_i\boxminus m\|^2 \]

协方差在切空间上计算：

\[ P_y = \sum_i W_i^c\,(\mathcal{Y}_i\boxminus\bar y)(\mathcal{Y}_i\boxminus\bar y)^\top \]

关键区别：所有差值 $\mathcal{Y}_i\boxminus\bar y$ 都在 $\bar y$ 的切空间 $\mathbb{R}^d$ 中，是合法向量，可以做外积。

实现参考¶

库	流形 UKF 支持	语言
UKF-M (Brossard et al. 2020)	$SO(3)$, $SE(3)$, $SE_2(3)$	Python/MATLAB
kalmanif (artivis)	基于 manif 的 $\boxplus/\boxminus$	C++ header-only
IKFoM (Xu et al. 2022)	一般流形 IEKF	C++ header-only
manif (Solà et al.)	提供 $\boxplus/\boxminus$ 底层	C++ header-only

UKF-M 的核心论文：Brossard, Barrau, Bonnabel, "A Code for Unscented Kalman Filtering on Manifolds (UKF-M)," ICRA 2020, DOI 10.1109/ICRA40945.2020.9197489。代码仓库：github.com/CAOR-MINES-ParisTech/ukfm。

预告与 A3 的连接¶

本节只给出了 $\boxplus/\boxminus$ 的接口定义和 UKF 的用法骨架。A3 将完整展开：

$SO(3)$、$SE(3)$、$SE_2(3)$ 上的 $\exp/\log$ 的完整推导
误差态 EKF (ESKF) 与 MEKF 的流形化
Invariant EKF (InEKF) 的状态无关 Jacobian
UKF-M 的完整实现与在 VIO 上的对比实验

本质洞察：$\boxplus/\boxminus$ 不是"给 UKF 打补丁"，而是重新定义了"状态空间中的加法和减法"。一旦接受这对算子，KF、EKF、UKF、CKF 的所有公式都可以在流形上重写——把 $+$ 换成 $\boxplus$，把 $-$ 换成 $\boxminus$，把协方差限制在切空间。这就是 Hertzberg 2013 和 Solà 2018 ("A micro Lie theory for state estimation in robotics," arXiv:1812.01537) 的核心思想。

练习¶

对两个四元数 $q_1$ 和 $q_2$（相差 90 度旋转），计算直接线性平均 $\frac{1}{2}(q_1+q_2)$ 的范数。解释为什么它不是单位四元数，以及归一化后为何不等于测地线中点。
写出 $\boxminus$ 的一维退化：当 $\mathcal{M}=\mathbb{R}$ 时，$y\boxminus x = y - x$。说明 $\boxplus/\boxminus$ 是普通加减法的推广。

§A2.22 CKF 与 GHKF：从数值积分角度看边界 ⭐⭐⭐⭐¶

这一节解决什么问题：把 CKF 和 GHKF 放到求积规则中理解，明确“精度阶”和“点数”之间的代价。

Gaussian filter 的关键积分是：

\[ I[g]=\int g(x)\mathcal{N}(x;\mu,P)\,dx \]

通过变量替换：

\[ x=\mu+S\xi \]

其中：

\[ SS^\top=P \]

积分变为标准高斯：

\[ I[g]=\int g(\mu+S\xi)\mathcal{N}(\xi;0,I)\,d\xi \]

CKF 使用球径分解。

把 $\xi$ 写成半径和方向：

\[ \xi=r s,\qquad \|s\|=1 \]

标准高斯是旋转对称的。

因此三阶规则只需要沿坐标轴正负方向取点：

\[ \xi_i=\sqrt n e_i,\qquad \xi_{n+i}=-\sqrt n e_i \]

权重：

\[ W_i=\frac1{2n} \]

所有权重为正。

没有中心点。

没有 $\alpha,\beta,\kappa$。

这就是 CKF 在高维下比典型 UKF 更少出现负协方差问题的原因。

GHKF 使用另一条路。

一维 Gauss-Hermite 求积选择 Hermite 多项式根作为节点。

$m$ 个节点可以对最高 $2m-1$ 阶多项式精确。

多维时最直接的做法是张量积：

\[ \{\xi_{i_1},\xi_{i_2},\dots,\xi_{i_n}\} \]

点数是：

\[ m^n \]

这不是实现细节。

这是 GHKF 的根本边界。

$m=5,n=3$ 时只有 125 个点。

$m=5,n=15$ 时是：

\[ 5^{15}=30,517,578,125 \]

实时机器人完全无法承受。

选型图¶

非线性弱且 Jacobian 易写
  └── EKF
非线性中等且 Jacobian 难写
  ├── n < 10: UKF
  └── n >= 10: CKF 或 EKF
低维且需要高阶矩精度
  └── GHKF / Sparse-grid GHKF
后验多峰或数据关联不确定
  └── GSF / Particle Filter / 多假设图优化
状态在 SO(3)/SE(3)
  └── ESKF / InEKF / UKF-M

练习¶

计算 $n=6,m=3$ 与 $n=15,m=3$ 的 GHKF 点数，解释为什么组合导航不常用张量积 GHKF。
证明 CKF 点集的一阶矩为 0、二阶矩为 $I$。
比较 CKF 与 UKF 在 $n=20$ 时的点数和权重符号风险。

§A2.23 非线性近似的边界：什么时候该离开 Kalman 族 ⭐⭐⭐⭐¶

这一节解决什么问题：把“滤波器发散”拆成可诊断的几类原因，避免只靠调 $Q,R$ 硬撑。

Kalman 族的共同前提是：

当前后验可以被一个均值和一个协方差描述。

当这个前提近似成立时，EKF/UKF/CKF 的区别主要是精度和计算代价。

当前提不成立时，换一个 sigma-point 规则也解决不了根因。

边界一：后验多峰¶

数据关联不确定时最常见。

例如移动机器人看到两个相似路标。

“我在左边路标附近”和“我在右边路标附近”都能解释量测。

真实后验有两个峰。

单高斯只能取中间。

中间位置可能反而是不可能的位置。

解决思路：

方法	适用条件	代价
Gaussian Sum Filter	峰数量有限	分量管理复杂
Particle Filter	低维、多峰明显	高维退化
多假设跟踪	数据关联核心问题	组合数量增长
因子图 + 鲁棒关联	SLAM 后端	需要批量优化

边界二：不连续或不可微¶

EKF 需要 Jacobian。

若观测函数包含最近邻匹配、阈值开关、接触切换，Jacobian 可能不存在或不稳定。

UKF/CKF 不需要 Jacobian，但仍假设函数传播后的点云能被均值协方差概括。

接触状态突然切换时，sigma points 可能一部分在接触、一部分在飞行。

平均结果不再对应真实物理状态。

解决思路是显式建模离散模式：

\[ p(x,m\mid y) \]

其中 $m$ 是接触模式、数据关联模式或运动模式。

这会进入 IMM、MHT、hybrid factor graph 等模型。

边界三：不可观测方向被错误收缩¶

SLAM/VIO 中，全局平移和全局 yaw 常常不可观。

滤波器应该沿这些方向保持不确定性。

标准 EKF 由于反复在变化的估计点线性化，会把这些方向误认为可观。

这不是简单调大 $Q$ 能根治的问题。

调大 $Q$ 只能让协方差更保守。

FEJ/OC-EKF/InEKF 才是在结构上处理不可观测子空间。

边界四：状态空间几何错误¶

四元数归一化后再直接加三维噪声，看似能运行。

但误差协方差实际活在切空间，不活在四维单位球面外的普通空间。

这会让协方差维度、约束和更新方向不一致。

解决思路是误差态滤波：

\[ R=\hat R\exp(\delta\theta^\wedge) \]

而不是：

\[ q=\hat q+\delta q \]

这部分属于 A3。

练习¶

举一个视觉 SLAM 中后验多峰的场景，并说明 EKF 会给出什么错误均值。
对腿式机器人接触切换，解释为什么单一连续 Gaussian filter 会在离地瞬间出现模型错配。
说明 VIO 的全局 yaw 为什么在没有磁力计或绝对航向观测时不可观。

§A2.24 机器人案例一：距离-方位传感器下的 EKF 定位 ⭐⭐⭐¶

这一节解决什么问题：用最经典的 range-bearing 观测展示 EKF Jacobian、NIS gate 和非线性边界。

状态：

\[ x=[p_x,p_y,\theta]^\top \]

已知路标：

\[ m=[m_x,m_y]^\top \]

定义：

\[ \Delta x=m_x-p_x \]

\[ \Delta y=m_y-p_y \]

\[ q=\Delta x^2+\Delta y^2 \]

距离观测：

\[ r=\sqrt q \]

方位观测：

\[ \phi=\mathrm{atan2}(\Delta y,\Delta x)-\theta \]

观测函数：

\[ h(x)= \begin{bmatrix} \sqrt q\\ \mathrm{atan2}(\Delta y,\Delta x)-\theta \end{bmatrix} \]

对状态求偏导。

距离对 $p_x$：

\[ \frac{\partial r}{\partial p_x} = \frac{1}{2\sqrt q}\cdot 2\Delta x\cdot\frac{\partial \Delta x}{\partial p_x} = -\frac{\Delta x}{\sqrt q} \]

距离对 $p_y$：

\[ \frac{\partial r}{\partial p_y} = -\frac{\Delta y}{\sqrt q} \]

距离对 $\theta$：

\[ \frac{\partial r}{\partial \theta}=0 \]

方位角对 $p_x$：

\[ \frac{\partial \phi}{\partial p_x} = \frac{\partial}{\partial p_x}\mathrm{atan2}(\Delta y,\Delta x) = \frac{\Delta y}{q} \]

方位角对 $p_y$：

\[ \frac{\partial \phi}{\partial p_y} = -\frac{\Delta x}{q} \]

方位角对 $\theta$：

\[ \frac{\partial \phi}{\partial \theta}=-1 \]

因此：

\[ H= \begin{bmatrix} -\Delta x/\sqrt q & -\Delta y/\sqrt q & 0\\ \Delta y/q & -\Delta x/q & -1 \end{bmatrix} \]

这个 Jacobian 暴露出危险点。

当机器人离路标很近时，$q\to0$。

第二行会变得巨大。

这不是数值偶然。

方位角在距离很近时本来就极其敏感。

一点点位置误差会造成很大角度变化。

工程上必须设置最小距离阈值或切换观测模型。

C++ 更新骨架¶

Eigen::Matrix<double, 2, 1> predictRangeBearing(
    const Eigen::Vector3d& x,
    const Eigen::Vector2d& landmark) {
  Eigen::Vector2d d = landmark - x.head<2>();
  double q = d.squaredNorm();
  Eigen::Matrix<double, 2, 1> zhat;
  zhat(0) = std::sqrt(q);                         // 预测距离
  zhat(1) = std::atan2(d.y(), d.x()) - x.z();     // 预测相对方位
  return zhat;
}

Eigen::Matrix<double, 2, 3> rangeBearingJacobian(
    const Eigen::Vector3d& x,
    const Eigen::Vector2d& landmark) {
  Eigen::Vector2d d = landmark - x.head<2>();
  double q = std::max(d.squaredNorm(), 1e-8);     // 防止距离过近导致除零
  double r = std::sqrt(q);

  Eigen::Matrix<double, 2, 3> H;
  H << -d.x() / r, -d.y() / r,  0.0,
        d.y() / q, -d.x() / q, -1.0;
  return H;
}

练习¶

用中心差分验证上面的 Jacobian，角度残差要做归一化。
解释为什么方位残差不能直接相减，而要 wrap 到 $(-\pi,\pi]$。
当 $q<10^{-4}$ 时拒绝该路标观测是否合理？给出你的理由。

§A2.25 机器人案例二：IMU+GNSS 组合导航中的 EKF/UKF 取舍 ⭐⭐⭐¶

这一节解决什么问题：把非线性滤波器选型放进组合导航工程场景。

典型 15 维误差态：

\[ \delta x= \begin{bmatrix} \delta p\\ \delta v\\ \delta\theta\\ \delta b_a\\ \delta b_g \end{bmatrix} \]

IMU propagation 高频运行。

GNSS 位置低频更新。

如果使用 EKF，优点是：

propagation Jacobian 有解析形式。
状态维度 15，矩阵规模适中。
每个 IMU 步只传播误差态协方差，计算快。
与误差态和李群姿态天然兼容。

缺点是：

需要认真推导姿态误差 Jacobian。
高动态、大初始姿态误差时一阶近似偏差明显。
时间同步和 bias 建模错误会被误认为量测噪声。

如果使用 UKF，优点是：

不必手写复杂 Jacobian。
大初始不确定性下均值传播通常更准。
对非加性噪声可用 augmented UKF。

缺点是：

15 维状态需要 31 个 sigma points。
若增广 IMU 噪声和 GNSS 噪声，点数进一步增加。
姿态平均必须在流形上处理，否则四元数均值会错。

因此工程选型常见结论是：

系统	推荐	原因
飞控主导航	ESKF/EKF	高频、低延迟、解析模型成熟
离线参数辨识	UKF/CKF	计算预算较宽，避免推导复杂 Jacobian
低维姿态估计	UKF/CKF	状态小，sigma points 代价低
VIO 前端	ESKF/MSCKF	状态增长、稀疏更新、FEJ 一致性
强非线性低维实验	GHKF	精度优先

如果不按状态几何处理会怎样？

直接把四元数当 4 维向量生成 sigma points。

传播后每个点都要归一化。

归一化是非线性投影，会改变点集的均值和协方差。

再把四元数分量线性平均，可能得到接近零范数的无效四元数。

这就是 UKF 在姿态问题上必须进入 A3 的原因。

练习¶

估算 15 维 additive UKF 与 15 维 EKF 每步需要调用多少次动力学函数。
若 augmented UKF 把 12 维 IMU 噪声也增广，sigma points 有多少个？
说明为什么 GNSS 位置更新本身是线性的，但整个组合导航仍是非线性的。

§A2.26 机器人案例三：腿式机器人接触辅助状态估计 ⭐⭐⭐¶

这一节解决什么问题：说明非线性滤波不只用于视觉和导航，也直接进入腿式机器人浮基估计。

四足机器人常用状态包括：

\[ x= \begin{bmatrix} R\\ p\\ v\\ b_g\\ b_a \end{bmatrix} \]

IMU 提供角速度和加速度。

腿部编码器通过正运动学给出足端相对机身位置。

若某只脚处于稳定接触，世界系足端速度近似为零。

这给浮基速度提供约束。

简化观测可以写成：

\[ 0 = v + R(\omega^\wedge r_{foot}+J_{leg}\dot q) +n \]

其中 $r_{foot}$ 是足端相对机身位置。

$J_{leg}\dot q$ 是足端相对机身速度。

这个观测对姿态 $R$、速度 $v$、陀螺仪 bias 都是非线性的。

EKF/ESKF 常用于这个场景。

关键困难不在公式本身，而在接触是否可靠。

若脚底打滑，却仍把足端速度当作零，滤波器会吸收错误约束。

表现为浮基速度突然变准、位置却慢慢漂到错误方向。

这时 NIS gate 可以检测一部分异常。

但滑动不是孤立外点。

它可能持续数百毫秒。

更稳健的做法是给接触概率建模：

\[ R_{contact}= \frac{1}{p_c}R_0 \]

接触概率 $p_c$ 低时，观测噪声增大。

这样滤波器不会完全相信可疑接触。

接触滤波的近似边界¶

情况	单一 EKF 是否合适	原因
稳定站立	合适	零速度接触约束强
平地小跑	合适但需接触判定	支撑相和摆动相切换
湿滑地面	需要鲁棒接触模型	零速度假设失效
跳跃腾空	接触观测应关闭	足端不再约束世界
踢到障碍物	需要外点处理	编码器运动学与实际接触冲突

这个案例连接到后续章节。

A3 会用流形误差处理姿态。

F 会用鲁棒估计处理滑动和冲击外点。

腿式控制章节会把估计出的浮基状态送入 WBC 和 MPC。

练习¶

写出足端零速度观测对浮基速度 $v$ 的 Jacobian。
说明接触概率降低时为什么应增大观测噪声 $R$。
设计一个 NIS gate，用于拒绝明显打滑的足端观测。

本章常见误解汇总¶

误解	正确理解
EKF 是 KF 的"升级版"，所有场景都应优先用 EKF	EKF 通过一阶 Taylor 近似处理非线性，在弱非线性/初值准确时表现好，但在强非线性或初值偏远时可能发散甚至比不做估计更差；它是"妥协"而非"升级"
UKF 比 EKF 一定更准	UKF 的 sigma 点传播均值精度到二阶、协方差精度到三阶，但在弱非线性场景下与 EKF 差异极小，且计算量更高（$2n+1$ 个 sigma 点）；当 Jacobian 容易解析求取时 EKF 往往是更经济的选择
CKF 是 UKF 的独立替代方案	CKF（三阶球面径向 Cubature 规则）在数学上是 Scaled UT 取 $\alpha=1,\beta=0,\kappa=0$ 的特例；它的优势在高维（$n>10$）时避免 UKF 中心权重为负的问题，而非一种全新方法
EKF-SLAM 的一致性问题可以通过"调参"解决	EKF-SLAM 的虚假可观测性是结构性问题：Jacobian 在估计点（而非真值）求值导致线性化后的可观性 Gramian 秩增大，人为注入了不存在的信息；这需要 FEJ/OC-EKF 等结构性修复，而非调 $Q/R$
Gauss-Hermite 滤波器（GHKF）精度最高所以应该首选	GHKF 可达任意阶精度，但 $p$ 阶积分需要 $p^n$ 个点（维度诅咒），在状态维度 $n>5$ 时计算量爆炸；实际机器人状态常 $n\ge 15$，GHKF 完全不可行
FEJ（First-Estimates Jacobian）会降低估计精度	FEJ 冻结 Jacobian 的线性化点确实放弃了"用最新估计更新 Jacobian"的机会，但这恰好保持了正确的可观性结构；实验表明 FEJ 在一致性和精度上通常同时优于无修正的 EKF
所有非线性滤波都是对 EKF 的线性递进改进	EKF/UKF/CKF/GHKF 是四种并行的逼近策略（Taylor 展开、sigma 点采样、球面径向积分、Gauss-Hermite 积分），各有适用场景，不存在简单的"越新越好"关系
粒子滤波可以完全替代 Kalman 族	粒子滤波不受高斯假设限制，但在高维状态空间（$n>6$）中需要指数级粒子数才能有效覆盖，实时性极差；Kalman 族在高斯/弱非线性场景下是不可替代的高效选择

§A2.27 本专题故障排查手册 ⭐⭐¶

症状	可能原因	排查步骤	推荐处理
EKF 初始几秒发散	初值太远，线性化点错误	打印残差、Jacobian 条件数、NEES	增大 $P_0$，改用 UKF/多阶段初始化
EKF 慢慢过自信	不可观测方向被错误收缩	检查 NEES、可观测子空间、FEJ 开关	使用 FEJ/OC-EKF/流形不变滤波
UKF Cholesky 偶发失败	协方差非正定或负中心权重影响	打印最小特征值、$W_0^c$、对称误差	对称化、加 jitter、用 SR-UKF 或 CKF
CKF 比 UKF 差	需要四阶矩修正或非线性强	对比同一数据的 NIS 与真实误差	调 UKF $\beta$ 或提高求积阶
GHKF 太慢	张量积点数爆炸	计算 $m^n$ 点数	改 sparse-grid 或 CKF
VIO yaw 协方差过快变小	标准 EKF 线性化破坏不可观测性	比较 `do_fej` 开关下 NEES	使用 FEJ 或 InEKF
Range-bearing 更新跳变	角度残差未归一化	打印残差是否超过 $\pi$	wrap 到 $(-\pi,\pi]$
腿式接触估计漂移	打滑仍作为零速度观测	看接触 NIS、足端速度残差	接触概率调噪声或关闭观测

排查的优先级是：

先确认残差定义是否正确。
再确认 Jacobian 或 sigma points 是否与状态误差定义一致。
再看 $Q,R,P_0$ 的物理单位。
最后判断是否已经超出单高斯滤波边界。

§A2.28 本专题小结补充：四种近似的来龙去脉 ⭐⭐¶

方法	历史动机	数学动作	最容易错的地方	机器人典型用法
EKF	把 KF 推广到弱非线性系统	在估计点 Taylor 一阶展开	Jacobian 点选错、不可观测方向过自信	MSCKF、robot_localization、飞控
UKF	避免手推 Jacobian，提高矩传播精度	用 sigma points 匹配均值协方差	参数导致负权重、姿态均值错误	中低维传感器融合
CKF	用数值积分构造全正权重点集	球径三阶求积	误以为总是优于 UKF	高维或不想调参的 Gaussian filter
GHKF	追求任意高阶多项式精度	Hermite 节点张量积	维度灾难	低维高精度姿态/过程估计

四者共同回答同一个问题：

\[ \mathbb{E}[g(x)],\quad \mathbb{E}[(g(x)-\mu_g)(g(x)-\mu_g)^\top] \]

如何在实时预算内近似。

答案没有绝对优劣。

只有与维度、非线性、几何结构、传感器频率和调试成本匹配的选择。

延伸阅读¶

类型	资源	难度	重点
教材	Särkkä & Svensson, Bayesian Filtering and Smoothing 2nd ed. (2023), Ch.7–8	⭐⭐	EKF/UKF/CKF 统一框架
教材	Simon, Optimal State Estimation (2006), Ch.13–14	⭐⭐⭐	EKF、二阶 EKF、IEKF 的经典推导
教材	Thrun et al., Probabilistic Robotics (2005), Ch.3, 7, 10	⭐⭐	机器人定位与 EKF-SLAM 入门
论文	Julier & Uhlmann, "Unscented Filtering and Nonlinear Estimation", Proc. IEEE 92(3):401–422, 2004	⭐⭐⭐	UKF 权威综述
论文	Arasaratnam & Haykin, "Cubature Kalman Filters", IEEE TAC 54(6):1254–1269, 2009	⭐⭐⭐	CKF 原始论文
论文	Huang, Mourikis, Roumeliotis, "Analysis and Improvement of the Consistency of EKF-based SLAM", ICRA 2008	⭐⭐⭐	FEJ/OC-EKF 一致性修正
论文	Hesch et al., "Consistency Analysis and Improvement of Vision-aided Inertial Navigation", IEEE T-RO 30(1), 2014	⭐⭐⭐⭐	VIO 不可观测性与 FEJ 在 3D 的推广
代码	`rpng/open_vins`	⭐⭐⭐	MSCKF + FEJ 工业级 VIO
代码	`cra-ros-pkg/robot_localization` (UKF 模式)	⭐⭐	ROS EKF/UKF 传感器融合
代码	`CAOR-MINES-ParisTech/ukfm`	⭐⭐⭐	UKF on Manifolds 参考实现

累积项目：本章新增模块¶

项目：Mini-State-Estimator 的非线性扩展

在 A1 的 Mini-State-Estimator 基础上，本章新增以下模块：

模块	功能	依赖
`ekf_nonlinear.py`	通用非线性 EKF（含解析/数值/自动微分 Jacobian 切换）	A1 KF 更新
`ukf_sigma.py`	UKF sigma 点生成、加权均值/协方差计算	A1 KF 框架
`ckf_cubature.py`	CKF 球面径向点集生成	ukf_sigma
`consistency_test.py`	Monte Carlo NEES/NIS 一致性检验 + $\chi^2$ 置信区间自动判定	A1 NEES

本章新增任务：

实现 ekf_nonlinear.py，用 range-bearing 观测模型在 2D 中跑 EKF，验证弱非线性下 NEES 仍在 $\chi^2$ 界内。
实现 ukf_sigma.py，用与上题相同数据跑 UKF，对比 EKF 在初始误差 $>30°$ 时的行为差异。
用 consistency_test.py 自动生成 50 次 Monte Carlo 的 ANEES 曲线，判定 EKF 在何种非线性水平下开始过自信。

§A2.29 跨章综合练习 ⭐⭐⭐¶

回到 A1 的 2D GPS+IMU 融合例子，把运动模型改成差速机器人非线性模型，推导 EKF 的 $F_k$，并保留 A1 的 Joseph form 与 NIS gate。
用同一组仿真数据分别跑 EKF、UKF、CKF。记录每一步 NIS，统计 95% $\chi^2$ gate 的拒绝率，解释三者差异。
把 range-bearing EKF 中的路标位置也加入状态，写出 EKF-SLAM 的状态结构，分析全局 $x,y,\theta$ 为什么不可观。
设计一个腿式机器人接触辅助滤波器：状态、过程模型、接触观测、接触概率调噪声、NIS gate 各写一段公式。
阅读 A3 前，尝试把姿态更新从普通加法改成 $R^+=R^-\exp(\delta\theta^\wedge)$，说明协方差仍应保存在三维切空间而不是九维矩阵空间。

§A2.30 带到 A3 的核心问题 ⭐⭐¶

A2 已经说明：

非线性可以用 Taylor 近似。

非线性积分可以用 sigma points、cubature points 或 Hermite points 近似。

但这些方法仍默认状态误差可以写成普通向量加法：

\[ x=\hat x+\delta x \]

对位置、速度、bias 这类欧氏变量，这没有问题。

对姿态和位姿，这个写法会破坏几何约束。

下一篇 A3 的入口问题就是：

\[ R=\hat R\exp(\delta\theta^\wedge) \]

和：

\[ T=\hat T\exp(\delta\xi^\wedge) \]

如何替代普通加法。

一旦误差定义改变，EKF/UKF 的公式表面相似，但 Jacobian、协方差、sigma points 的生成与回收全部要在切空间完成。

这就是从“经典非线性滤波”进入“流形滤波”的分界线。

常见陷阱与故障排查¶

⚠️ 陷阱一：跨教材抄公式时对调 $Q/R$。 有的教材用 $R$ 表示运动噪声、$Q$ 表示观测噪声；本文统一采用 $Q$ 为过程噪声、$R$ 为观测噪声。

⚠️ 陷阱二：把 UKF 的“免 Jacobian”理解成“适合所有非连续函数”。 sigma 点可以穿过不连续面，但单高斯矩匹配会被模式切换破坏。

⚠️ 陷阱三：把 IEKF 和 InEKF 写成同一个缩写。 IEKF 是 iterated EKF，解决强非线性量测的迭代线性化；InEKF 是 invariant EKF，解决群结构和可观性一致性。

故障排查现象	可能原因	处理方式
EKF 在大初值误差下发散	一阶线性化过远	尝试 IEKF/LM 或重新初始化
UKF 协方差不正定	权重负值或 sigma 点过远	调大 $\alpha$、检查 Cholesky jitter
接触/数据关联切换时滤波跳变	单一连续模型描述离散模式	使用 IMM、PF 或混合因子图

本质洞察：A2 的所有方法都在回答同一个问题：当 $f,h$ 非线性时，如何近似传播一个高斯分布；区别只在于用 Taylor、sigma points 还是迭代 MAP 来近似。

练习¶

选一个标量非线性函数 $y=x^2$，比较 EKF 和 UKF 对均值、方差的近似误差。
构造一个带阈值切换的观测函数，观察 UKF sigma 点跨过阈值时均值如何偏移。
把同一量测更新写成 EKF 一次线性化和 IEKF 多次线性化，说明二者与 Gauss-Newton 的关系。

场景	\(\alpha\)	\(\beta\)	\(\kappa\)	备注
姿态估计（3 维 sigma）	\(10^{-3}\)	2	0	跟随 Wan-Van der Merwe 默认
组合导航（IMU+GNSS，15 维）	\(10^{-3}\) 到 \(10^{-2}\)	2	0	太小 \(\alpha\) 会让 \(W^c\) 过大致病态
EKF-SLAM 替代 UKF-SLAM	\(10^{-2}\)	2	\(3-n\)	高维下 \(3-n\) 会让 \(W_0^m<0\)，加 Joseph form 保护
van der Pol / Lorenz 数值实验	1	2	0	\(\alpha=1\) 退化为原始 Julier UT
简单 1-2 维非线性	1	0	2	传统 Julier 选择

编号	问题	答不出时回顾
1	写出线性 KF 的预测和更新公式（含 Joseph form）。Kalman 增益 \(K\) 的两个极限行为是什么？	A1 §A1.5
2	信息矩阵 \(\Lambda=P^{-1}\) 的更新公式为什么只有加法？解释其工程意义。	A1 §A1.9-A1.10
3	NEES 的定义是什么？\(\varepsilon_k\sim\chi^2_n\) 成立的前提条件是什么？"超出上界"意味着什么？	A1 §A1.15-A1.16
4	Woodbury 恒等式如何把信息形 KF 变换为协方差形 KF？写出关键步骤。	A1 §A1.7
5	平方根滤波器为什么能"翻倍有效精度"？写出条件数关系式。	A1 §A1.12

维度	EKF	UKF
近似对象	非线性函数 \(g\) 一阶 Taylor	输入分布 \(p(x)\) 的 sigma 采样
Jacobian	必须	不需要
均值精度	1 阶	3 阶（对称对消）
协方差精度	1 阶	2 阶（\(\beta=2\) 时 4 阶矩对齐）
主循环复杂度	\(O(n^3)\)	\(O(n^3)\)（Cholesky 主导）+ \(2n+1\) 次 \(f/h\) 调用
对强非线性鲁棒性	差（发散风险）	优
对大初始不确定性鲁棒性	差	优
对不连续函数	失效（Jacobian 不存在）	可直接评估，但通常不可靠；应显式建模离散模式
易用性	推 Jacobian 繁琐	只需 \(f,h\) 即可
工业成熟度	★★★★★（30 年）	★★★★（20 年）

滤波器	积分规则	点数	精度（高斯下）
EKF	一阶 Taylor（伪积分）	1（\(\mu\) 本身）	1
二阶 EKF	二阶 Taylor	1+Hessian	2
UKF	Scaled UT（球径-径向）	2n+1	均值 3，协方差 2
CKF	3 阶球径 + 1 阶径向	2n	3
5 阶 CKF	5 阶球径	\(2n^2+1\)	5
GHKF(m)	m 阶 Gauss-Hermite 张量积	m^n	2m-1
Sparse-Grid GHKF(L)	Smolyak 稀疏网格	O(n^L)	约 2L-1

Sigma 集	点数	权重	精度阶	适用维度	来源
Julier Symmetric Set（原始 UT）	2n+1	含 \(W_0=\kappa/(n+\kappa)\)	均值 3	低-中	Julier-Uhlmann 1997
Merwe Scaled Set	2n+1	由 \(\alpha,\beta,\kappa\) 参数化	均值 3，\(\beta=2\) 时匹配 4 阶矩	低-高	Van der Merwe PhD 2004
Simplex Sigma Points	n+1	非对称	仅均值	嵌入式低算力	Julier 2003
Spherical-Radial Cubature	2n	全部 1/(2n)	3	中-高	Arasaratnam-Haykin 2009
5 阶 Cubature	\(2n^2+1\)	混合权	5	中维	Jia-Xin-Cheng 2013
Gauss-Hermite Tensor Product	m^n	Hermite 多项式根	2m-1 每维	仅 \(n \le 4\)	Ito-Xiong 2000
Smolyak Sparse Grid	O(n^L)	含负权	可调	中-高	Smolyak 1963 / Jia 2012

方法	近似对象	近似方式	代价
EKF	函数 \(g\)	一阶 Taylor	需要 Jacobian
二阶 EKF	函数 \(g\)	二阶 Taylor	需要 Hessian
UKF	分布的矩	2n+1 个 sigma points	需要调 \(\alpha\)/\(\beta\)/\(\kappa\)
CKF	高斯积分	2n 个球径点	无调参
GHKF	高斯积分	Hermite 张量积	点数 \(m^n\)
Sparse-grid	高斯积分	Smolyak 稀疏网格	实现更复杂

库	流形 UKF 支持	语言
UKF-M (Brossard et al. 2020)	\(SO(3)\), \(SE(3)\), \(SE_2(3)\)	Python/MATLAB
kalmanif (artivis)	基于 manif 的 \(\boxplus/\boxminus\)	C++ header-only
IKFoM (Xu et al. 2022)	一般流形 IEKF	C++ header-only
manif (Solà et al.)	提供 \(\boxplus/\boxminus\) 底层	C++ header-only

20_经典非线性滤波族

博士前数学路线图·第五批·子专题 A2：经典非线性滤波族——EKF、UKF、CKF 与 GHKF¶

前置自测 ⭐¶

本章目标¶

为什么你必须学这个专题¶

§A2.1 从 KF 到非线性：EKF 的一阶线性化 ⭐⭐¶

动机与定位¶

一阶 Taylor 线性化¶

为什么 EKF 不是最优贝叶斯滤波器¶

§A2.2 EKF 的严格推导与算法流程 ⭐⭐¶

预测步¶

更新步¶

教材交叉定位¶

算法伪码（Thrun Table 3.3 结构，按本文 \(Q/R\) 约定改写）¶

EKF 为什么会发散——几何直觉¶

练习¶

§A2.3 EKF Jacobian 的三种计算方式 ⭐⭐¶

§A2.4 EKF 一致性问题：不可观测方向与虚假信息 ⭐⭐⭐¶

什么是"一致性"¶

EKF-SLAM 的"虚假信息获取"¶

几何直觉¶

练习¶

§A2.5 First-Estimates Jacobian (FEJ) 与 Observability-Constrained EKF ⭐⭐⭐¶

FEJ：把线性化点钉死¶

OC-EKF：投影到不可观测子空间的补¶

MSCKF 2.0 的一致性修正¶

伏笔：为什么 IEKF 能从根本上解决¶

§A2.6 Unscented Transform：确定性采样传播 ⭐⭐¶

动机¶

数学推导（对称 2n+1 点集）¶

\(\alpha\), \(\beta\), \(\kappa\) 的含义¶

Scaled UT 为什么必要¶

UT 精度阶——三阶证明要点¶

线性退化¶

练习¶

§A2.7 UKF 完整推导与参数选择 ⭐⭐¶

预测步（Additive 形式）¶

更新步¶

Additive vs Augmented UKF¶

UKF vs EKF 对比¶

\(\alpha\)/\(\beta\)/\(\kappa\) 速查表¶

§A2.8 Square-Root UKF 与数值稳定性 ⭐⭐⭐¶

Cholesky 失败的根源¶

工程救援手段¶

Square-Root UKF（Van der Merwe & Wan, ICASSP 2001, DOI 10.1109/ICASSP.2001.940586）¶

§A2.9 Cubature Kalman Filter (CKF) ⭐⭐⭐¶

动机¶

三阶球径求积规则¶

CKF 算法¶

CKF = UKF(\(\alpha=1, \beta=0, \kappa=0\)) 的证明¶

CKF vs UKF¶

练习¶

CKF 衍生¶

§A2.10 Gauss-Hermite Kalman Filter 与 Sparse-Grid ⭐⭐⭐⭐¶

GHKF（Ito & Xiong, IEEE TAC 45(5), 2000, DOI 10.1109/9.855552）¶

多维张量积——维度灾难¶

Sparse-Grid Quadrature（SGQ）¶

统一视角¶

§A2.11 迭代变体：IEKF 与 ISPKF ⭐⭐⭐⭐¶

IEKF：作为 Gauss-Newton（Bell & Cathey, IEEE TAC 38(2), 1993, DOI 10.1109/9.250476）¶

ISPKF（Iterated Sigma-Point KF；Barfoot 2ed §4.2.10, p.137）¶

与 Bundle Adjustment 的桥¶

§A2.12 实战：MSCKF 与 OpenVINS 的 EKF 工程 ⭐⭐¶

MSCKF 状态结构（Mourikis-Roumeliotis, ICRA 2007, DOI 10.1109/ROBOT.2007.364024；PDF: www-users.cse.umn.edu/~stergios/papers/ICRA07-MSCKF.pdf）¶

Null-space Projection（消元特征点）¶

OpenVINS 源码地图（github.com/rpng/open_vins，UDel RPNG Huang 组）¶

FEJ 在 OpenVINS 中的开关¶

JPL vs Hamilton 四元数¶

OpenVINS 论文与性能¶

VINS-Mono 作为滤波-优化对照¶

ROS robot_localization（Moore & Stouch, IAS-13, 2014）¶

ROVIO（ETH ASL，对照）¶

§A2.13 高斯和滤波器与粒子滤波：Kalman 族的边界 ⭐⭐⭐¶

Gaussian Sum Filter（GSF）¶

Particle Filter（PF）¶

Rao-Blackwellized Particle Filter（RBPF / FastSLAM）¶

§A2.14 汇总：非线性滤波族全景对比 ⭐⭐¶

Sigma Point 家族对比¶

关键公式速查¶

必读论文表¶