EKF-SLAM：理解现代后端为何要图优化

1. 为何仍值得学

Smith & Cheeseman 1987 的 EKF-SLAM 把机器人位姿与所有路标放进状态向量，来观测就 EKF 更新。今天主流视觉/LiDAR SLAM 都用图优化，但 EKF-SLAM 没有被扫进历史垃圾堆——理解它的失败模式，等于理解现代 SLAM 后端为什么要那样设计。

2. 状态增广与复杂度

x_{t} = [x_{t}^{r},, l_{1}, \dots, l_{M}]^{⊤}

$x^{r}$ 是机器人位姿， $l_{j}$ 是路标。每观测新路标，状态维数 +2 或 +3，协方差 $P$ 线性增长。1000 路标 → 3000+ 维，更新 $O (n^{3})$ 不可承受——这是被图优化取代的首要原因。

3. EKF 更新循环

预测： $x^t ∣ t - 1 = f (x^t - 1, u_{t})$ ， $P_{t ∣ t - 1} = F P F^{⊤} + Q$ 。

更新： $K = P H^{⊤} (H P H^{⊤} + R)^{- 1}$ ， $x^\leftarrow x^+ K (z - h (x^))$ ， $P \leftarrow (I - K H) P$ 。

关键：更新整块 $P$ ，含位姿-路标、路标-路标交叉相关——不能假设路标独立。这是 EKF-SLAM 与「独立建图」的本质区别。

4. 不一致性

Julier & Uhlmann 指出：EKF 对非线性系统线性化不一致，协方差过于乐观。SLAM 中表现为认为定位很准、实际漂移大；新观测权重被高估，旧信息被过快压缩。Joseph form、IEKF 能缓解不能根治。图优化通过反复 relinearize 和全局一致性约束解决——代价是延迟。Bailey 等人的 EKF-SLAM inconsistency 实验表明：同样轨迹下 EKF 估计误差可达协方差椭圆数倍——读 VIO 论文里「协方差可信」要先问后端是滤波还是因子图。

5. 数据关联

EKF-SLAM 假设数据关联已知（哪个观测对应哪个路标）。关联错 → 路标被拉到错误位置 → 不可恢复。现代 SLAM 用 RANSAC、JCBB、图匹配——独立于滤波/图优化选择的模块。

6. 今天 EKF 仍在哪里

ESKF/IEKF：VINS-Mono、OpenVINS、ROVIO 前端——本质是 EKF，状态维数固定，路标逆深度参数化在外部维护。低维定位：AMCL 是粒子滤波；GPS/IMU 松耦合融合也常见 EKF。教学： $P$ 的特征值直接反映不可观方向，读 VIO 可观性论文最自然。误差状态在 IMU 名义状态附近线性化，名义状态用 IMU 积分传播——这是现代 VIO 能在固定维数下保持 100Hz 的关键，而不是把路标塞进 $x$ 。

FastSLAM 用粒子表示位姿、每粒子独立 EKF 路标——绕开 $O (n^{2})$ 更新，但粒子数与路标数仍打架；3D SLAM 主流仍是图优化。粒子方法更多留在 2D 定位（AMCL），理解 FastSLAM 有助于读「为什么 EKF-SLAM scale 不了但粒子可以部分绕开」——本质是计算换一致性。

7. EKF → 图优化对应

EKF-SLAM	图优化
状态增广	新增变量节点
EKF 更新	新增因子 + 优化
协方差 $P$	Hessian 逆（信息矩阵）
线性化点固定	反复 relinearize
$O (n^{3})$ 更新	稀疏 $O (n)$ Cholesky

早期 sparsification 试图近似丢弃交叉相关——理论 messy，实践不如转图优化；读老论文时不被「EKF 也能 scale」误导。

学完 EKF-SLAM 再读 g2o：状态增广对应加节点，EKF 更新对应加因子，协方差 $P$ 对应 Hessian 逆——三条映射清楚后，滑窗边缘化、FEJ 讨论才不会像黑魔法。数据关联模块选 JCBB 还是 RANSAC，与后端滤波/图优化正交，但 EKF 时代就教会我们：关联错比更新慢更致命。Joseph form 更新 $P$ 在 teaching 代码里值得实现一次，亲眼看到 trace 与 ATE 背离比读论文更直观。

8. 验收（若做实验）

状态维数 vs 更新耗时
协方差 trace vs 实际 ATE（一致性检验）
数据关联错误率

EKF-SLAM 是 SLAM = 估计 + 关联 + 地图表示最干净的入口；学完再读 g2o/GTSAM，知道每条边在修 EKF 的哪个具体痛点。若做教学实验，优先在小规模（<50 路标）上对比 EKF trace 与 ground truth ATE——不一致性在规模放大前就能看见，比直接上千路标更容易讲清楚为什么 industry 转图优化。