跳到主要内容

状态估计

为什么重要​

机器人不知道自己在哪，一切 3D 表示都无处安放。状态估计是空间智能的推断引擎：把带噪传感器流变成连续可信的位姿与地图估计。本模块与 路线 A 模块 03 互为镜像——那里学理论，这里学系统。

直观理解​

Kalman filter

预测-更新循环在机器人语境下的含义：IMU/轮速计把 belief 向前传播（不确定性膨胀），相机/GPS 观测把它拉回（不确定性收缩）。椭圆是协方差——机器人对自身位置"把握有多大"。

核心思想​

空间智能的状态估计面对的现实是非线性 + 多传感器 + 实时。非线性来自旋转与透视投影：EKF 在当前估计处做一阶泰勒展开线性化，简单但对大误差发散；UKF 用少量 sigma 点传播分布，无需雅可比且精度到二阶。两者都是"滤波"路线——只维护当前状态的 belief，历史被边际化掉。

优化路线（smoothing）则保留整段轨迹：把全部测量写成因子图——变量节点是位姿与路标，因子节点是测量约束——求解整个图上的非线性最小二乘。因子图视角统一了一切：EKF 是只保留最新节点的因子图，滑窗 VIO 是固定窗口的因子图，SLAM 后端是完整的因子图。MIT VNAV 的 L16–L19（ML/MAP 估计 → 非线性最小二乘）把这条推导链讲得最透。

工程要点是不确定性的正确使用：协方差不只是输出，它决定传感器融合的权重（噪声大的传感器自动降权）、决定何时信任模型（ Kalman gain 的空间版）、决定下游规划的安全余量。VIO（模块 07）就是本模块理论 + IMU 预积分 + 视觉前端的系统集成。

关键概念​

  • EKF：局部线性化的滤波，简单脆弱但无处不在。
  • UKF：sigma 点无迹变换，免雅可比、二阶精度。
  • 因子图：变量 + 测量约束的图模型，SLAM/VIO 的统一语言。
  • 滤波 vs 平滑：边际化历史 vs 联合优化整段轨迹。
  • 协方差即权重：不确定性驱动传感器融合与规划安全边际。

核心公式​

EKF 的线性化更新（Ft,HtF_t, H_t 为雅可比）：

Pt∣t−1=FtPt−1∣t−1Ft⊤+Q,Kt=Pt∣t−1Ht⊤(HtPt∣t−1Ht⊤+R)−1P_{t|t-1} = F_t P_{t-1|t-1} F_t^\top + Q, \qquad K_t = P_{t|t-1} H_t^\top (H_t P_{t|t-1} H_t^\top + R)^{-1}

因子图的 MAP 目标：

X∗=arg⁡min⁡X∑i∥hi(X)−zi∥Σi2X^* = \arg\min_X \sum_i \big\| h_i(X) - z_i \big\|_{\Sigma_i}^2

大学课程​

课程Lecture链接
Stanford CS231AL14–L15 Optimal Estimation（KF/EKF/UKF，flipped format）+ PS4课程主页
MIT 16.485 VNAVL16–L19 ML/MAP Estimation 与非线性最小二乘课程主页

论文​

  • Must Read：Kalman (1960), A New Approach to Linear Filtering and Prediction Problems（经典论文，搜索论文名）。
  • Recommended：Thrun, Burgard & Fox, Probabilistic Robotics（教材，搜索书名）——机器人状态估计的圣经，第 3 章。
  • Optional：dynamax（JAX 生态的 SSM 库）与 GTSAM（因子图优化库）的 tutorial。

动手实践​

在 Colab 中打开在 Colab 中打开: lab01_kalman_filter

Lab 1 手写 KF 跟踪 2D 目标并与 dynamax 对比。路线 B 学习者额外建议：把观测换成"只在某些时刻可用"（模拟 GPS 丢失），观察协方差的膨胀与收缩。

自测​

  1. EKF 在什么情况下会发散？
  2. 因子图视角下，EKF 与全平滑 SLAM 的区别是什么？
  3. 为什么 IMU 与视觉是互补的传感器？
显示答案
  1. 当初始误差大到一阶线性化失效时（旋转大角度、强非线性观测），线性化点远离真实后验模式，更新方向错误，误差正反馈导致发散。UKF/迭代 EKF/优化路线都是对此的修补。
  2. EKF 每步把历史边际化进当前 belief，等价于只保留最新变量的因子图，信息一旦丢弃无法恢复；全平滑保留全部变量与约束联合优化，精度高但计算贵。滑窗优化是两者的折中。
  3. IMU 高频、短期精确但漂移（积分噪声累积）；视觉低频、无漂移但有尺度歧义与纹理依赖。融合后 IMU 提供帧间高频约束、视觉提供长期锚定并恢复尺度——这正是 VIO 的设计逻辑（模块 07）。

要点回顾​

  • 状态估计 = 带噪传感器流 → belief 的递归/批量推断，EKF/UKF/因子图是三代工具。
  • 滤波与平滑在因子图语言下统一，滑窗是工程折中。
  • 协方差是融合权重与安全余量的来源——不确定性是系统的燃料而非附属品。

下一模块​

模块 07：SLAM 与 VIO——把状态估计组装成完整的定位建图系统。