高斯滤波 — KF, EKF, UKF, IF

  • Description：Probabilistic Robotics 第 3 章读书笔记 — 高斯分布假设下的 Bayes 滤波：线性最优 KF、非线性 EKF (Taylor 一阶展开)、UKF (Sigma 点无迹变换)、信息滤波 IF (canonical 形式)
  • My Notion Note ID：K2E-B-B1-3
  • Created：2026-06-06
  • Updated：2026-06-06
  • License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

Table of Contents

1. 高斯假设

高斯滤波假设 bel(xt)=N(μt,Σt)\text{bel}(x_t) = \mathcal{N}(\mu_t, \Sigma_t)，即置信度始终是高斯分布。

两种等价参数化：

表示 参数 适合场景
矩形式 (moments) (μ,Σ)(\mu, \Sigma) KF / EKF / UKF — 预测步方便
正则形式 (canonical) (Ω,ξ)(\Omega, \xi)，Ω=Σ−1\Omega = \Sigma^{-1}，ξ=Σ−1μ\xi = \Sigma^{-1}\mu IF — 更新步方便，稀疏结构

2. 卡尔曼滤波 (KF)

适用：线性系统 + 高斯噪声 → 最优滤波（最小均方误差）。

线性模型：xt=Atxt−1+Btut+ϵtx_t = A_t x_{t-1} + B_t u_t + \epsilon_t，zt=Ctxt+δtz_t = C_t x_t + \delta_t，ϵt∼N(0,Rt)\epsilon_t \sim \mathcal{N}(0, R_t)，δt∼N(0,Qt)\delta_t \sim \mathcal{N}(0, Q_t)。

预测步： μˉt=Atμt−1+Btut\bar{\mu}_t = A_t \mu_{t-1} + B_t u_t Σˉt=AtΣt−1AtT+Rt\bar{\Sigma}_t = A_t \Sigma_{t-1} A_t^T + R_t

卡尔曼增益： Kt=ΣˉtCtT(CtΣˉtCtT+Qt)−1K_t = \bar{\Sigma}_t C_t^T (C_t \bar{\Sigma}_t C_t^T + Q_t)^{-1}

更新步： μt=μˉt+Kt(zt−Ctμˉt)\mu_t = \bar{\mu}_t + K_t(z_t - C_t \bar{\mu}_t) Σt=(I−KtCt)Σˉt\Sigma_t = (I - K_t C_t)\bar{\Sigma}_t

Kt(zt−Ctμˉt)K_t(z_t - C_t\bar{\mu}_t) 中括号内是新息 (innovation)，即测量与预测的差；KtK_t 决定权重（测量 vs 预测）。

性质：线性高斯下 KF = MAP = 最小方差估计 = 最大似然。

3. 扩展卡尔曼滤波 (EKF)

非线性系统：xt=g(ut,xt−1)+ϵtx_t = g(u_t, x_{t-1}) + \epsilon_t，zt=h(xt)+δtz_t = h(x_t) + \delta_t。

对非线性函数在当前估计处做一阶 Taylor 展开：

Gt=∂g∂xt−1∣μt−1,ut,Ht=∂h∂xt∣μˉtG_t = \frac{\partial g}{\partial x_{t-1}}\bigg|_{\mu_{t-1}, u_t}, \quad H_t = \frac{\partial h}{\partial x_t}\bigg|_{\bar{\mu}_t}

用 GtG_t 替换 KF 的 AtA_t，用 HtH_t 替换 CtC_t，其余结构与 KF 相同。

局限：

  • 线性化带来近似误差；高度非线性时协方差可能低估（过乐观）
  • 需要解析 Jacobian（代码负担，EKF-SLAM 中路标多时 JJ 计算量大）
  • 不保证单峰分布在变换后仍是高斯（全局定位多峰时 EKF 失败）

4. Unscented KF (UKF)

思路：用 2n+12n+1 个确定性 Sigma 点精确传播均值和协方差，不做 Taylor 展开。

Sigma 点（nn = 状态维数，λ=α2(n+κ)−n\lambda = \alpha^2(n + \kappa) - n）：

X[0]=μ\mathcal{X}^{[0]} = \mu X[i]=μ+((n+λ)Σ)i,i=1,…,n\mathcal{X}^{[i]} = \mu + \left(\sqrt{(n+\lambda)\Sigma}\right)_i, \quad i = 1,\ldots,n X[i]=μ−((n+λ)Σ)i−n,i=n+1,…,2n\mathcal{X}^{[i]} = \mu - \left(\sqrt{(n+\lambda)\Sigma}\right)_{i-n}, \quad i = n+1,\ldots,2n

((n+λ)Σ)i\left(\sqrt{(n+\lambda)\Sigma}\right)_i 是矩阵平方根的第 ii 列（通常用 Cholesky 分解）。

传播每个 Sigma 点通过非线性函数，加权求均值和协方差：

μˉt≈∑i=02nWm[i]g(X[i])\bar{\mu}_t \approx \sum_{i=0}^{2n} W_m^{[i]} g(\mathcal{X}^{[i]}) Σˉt≈∑i=02nWc[i](g(X[i])−μˉt)(g(X[i])−μˉt)T+Rt\bar{\Sigma}_t \approx \sum_{i=0}^{2n} W_c^{[i]} (g(\mathcal{X}^{[i]}) - \bar{\mu}_t)(g(\mathcal{X}^{[i]}) - \bar{\mu}_t)^T + R_t

优点：对多项式非线性精确到 3σ3\sigma（比 EKF 高一阶）；不需要解析 Jacobian。缺点：2n+12n+1 次函数调用；高维度时 Sigma 点数量大。

5. 信息滤波 (IF)

KF 的 canonical 形式，用信息矩阵 Ωt=Σt−1\Omega_t = \Sigma_t^{-1} 和信息向量 ξt=Σt−1μt\xi_t = \Sigma_t^{-1}\mu_t。

更新步（简单 — 加法）： Ωt=Ωˉt+HtTQt−1Ht\Omega_t = \bar{\Omega}_t + H_t^T Q_t^{-1} H_t ξt=ξˉt+HtTQt−1zt\xi_t = \bar{\xi}_t + H_t^T Q_t^{-1} z_t

预测步（复杂 — 矩阵求逆）：需先恢复矩形式，才能应用运动模型。

适合：稀疏信息矩阵（路标独立性强时 Ω\Omega 是块稀疏）、多传感器融合（信息可直接相加）、去中心化估计（各节点维护局部信息，融合只需加法）。

References

  • Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics. MIT Press, 2005. 第 3 章 — 本笔记内容来源