Description：粒子滤波全景 — 非参数 Bayes 递推：重要性采样、SIS 退化根因、SIR 重采样修复、重采样策略对比 (multinomial/systematic/residual/stratified)、粒子退化与贫化、MCL 定位与 FastSLAM/Rao-Blackwellized PF、与高斯滤波的取舍
My Notion Note ID：K2E-A-E4
Created：2023-04-10
Updated：2026-06-06
License：转载欢迎 — 请署名 Yu Zhang 并链回 yuzhang.io 原文

1. 为什么需要粒子滤波
2. 重要性采样 (Importance Sampling)
3. SIS - Sequential Importance Sampling
4. SIR - Sequential Importance Resampling
5. 重采样策略
6. 粒子退化与贫化
7. SLAM 中的应用
8. 变体
9. 速查对比
References

1. 为什么需要粒子滤波

Bayes 滤波的通用形式：

\text{bel}(x_t) = \eta \cdot p(z_t | x_t) \int p(x_t | x_{t-1}, u_t) \, \text{bel}(x_{t-1}) \, dx_{t-1}

具体化：

假设	滤波器	限制
线性 + 高斯	KF	严格线性
弱非线性 + 高斯	EKF	Jacobian 近似可能发散
强非线性 + 高斯	UKF / iEKF	仍是高斯假设
任意分布 + 任意非线性	粒子滤波 (PF)	维数爆炸

粒子滤波 = 用一堆带权样本逼近任意后验分布。不要求线性，也不要求高斯。

1.1 适用 SLAM 场景

机器人全局定位 — 后验分布可能多峰 (对称环境 → 多个候选位姿)，高斯不适用
绑架问题 — 跟踪丢失后需要全局重定位，粒子能分散覆盖整个状态空间
FastSLAM — 路径用 PF，地图特征用 KF (Rao-Blackwellized 分解)
激光 + Occupancy Grid 定位 (蒙特卡洛定位， MCL)

2. 重要性采样 (Importance Sampling)

问题：要估计 $E_{p}[f(x)] = \int f(x) p(x) \, dx$ ，但 $p(x)$ 无法直接采样。

思路：从一个容易采样的提议分布 $q(x)$ 抽样，加权修正：

E_p[f(x)] = \int f(x) \frac{p(x)}{q(x)} q(x) \, dx = E_q\left[ f(x) \cdot \frac{p(x)}{q(x)} \right]

抽 $N$ 个样本 $x_i \sim q(x)$ ，权重 $w_i = p(x_i) / q(x_i)$ ：

E_p[f(x)] \approx \frac{1}{N} \sum_i w_i f(x_i)

如果 $p$ 只能算到归一化常数差 (未归一化)，用归一化重要性采样：

E_p[f(x)] \approx \frac{\sum_i w_i f(x_i)}{\sum_i w_i}

关键 takeaway：通过 $q$ 抽样 + 权重修正，效果等价于从原本未知的 $p$ 直接抽样。

3. SIS - Sequential Importance Sampling

把重要性采样推广到时序状态估计：

状态 $x_{0:t}$ (路径)，观测 $z_{1:t}$
后验 $p(x_{0:t} | z_{1:t})$ 难算
选提议分布 $q(x_{0:t} | z_{1:t})$ 容易采样

3.1 增量形式

如果 $q$ 满足马尔可夫性 $q(x_{0:t}) = q(x_t | x_{0:t-1}, z_{1:t}) \cdot q(x_{0:t-1})$ ，权重可递推：

w_t^{(i)} \propto w_{t-1}^{(i)} \cdot \frac{p(z_t | x_t^{(i)}) \, p(x_t^{(i)} | x_{t-1}^{(i)})}{q(x_t^{(i)} | x_{0:t-1}^{(i)}, z_{1:t})}

每来一帧观测，新采一个 $x_t^{(i)} \sim q(\cdot)$ 添到路径末尾，权重按上式更新。

3.2 常见提议分布

最简单：用动力学模型作为提议：

q(x_t | x_{0:t-1}, z_{1:t}) = p(x_t | x_{t-1}, u_t)

权重退化为：

w_t^{(i)} \propto w_{t-1}^{(i)} \cdot p(z_t | x_t^{(i)})

即"权重 = 上一时刻权重 × 当前观测似然"。直观、易实现，但容易粒子退化 (见 §6)。

4. SIR - Sequential Importance Resampling

SIS 的最大问题：随时间推移，少数粒子权重指数级增大，大部分粒子权重趋零 — 粒子退化 (degeneracy)。

解决：每次 SIS 更新后，重采样一次 — 按权重抽样，权重大的粒子被复制多次，权重小的被丢弃。

4.1 SIR 算法

初始化: N 个粒子 {x_0^(i)}, 等权重 w_0^(i) = 1/N

每个时刻 t:
    1. 预测 (运动方程):
       对每个 i: x_t^(i) ~ p(x_t | x_{t-1}^(i), u_t)
    2. 权重更新 (观测方程):
       w_t^(i) ∝ p(z_t | x_t^(i))
       归一化: w_t^(i) = w_t^(i) / Σ w_t^(j)
    3. 估计:
       x_t ≈ Σ w_t^(i) x_t^(i)  (加权均值；或取权重最大粒子)
    4. 重采样:
       从 {x_t^(i)} 按 w_t^(i) 抽 N 个新粒子，等权重

重采样后 SIR 与 SIS 的区别只是步 4。但效果差很多 — SIR 维持有效粒子数。

4.2 SIR 与 SIS 的关系

"其思路就是我原本的采样分布是不知道的，我如何从一个已知的分布中采样，通过加权的方式使得从已知的分布中采样的粒子分布和原本未知的分布中采样的粒子分布结果一致 — 从而引入 SIS 粒子滤波，再进一步加入重采样后就引入了 SIR 粒子滤波。"

5. 重采样策略

按权重抽样的具体算法：

算法	思路	复杂度	方差
多项式重采样 (Multinomial)	$N$ 次独立按权重抽样	$O(N \log N)$	高
系统重采样 (Systematic)	等距 + 一个随机偏移	$O(N)$	低 (推荐)
分层重采样 (Stratified)	划 $N$ 个等概率区间，每区间抽 1	$O(N)$	低
残差重采样 (Residual)	整数部分直接复制 + 小数部分多项式	$O(N)$	低 (与分层同属可证优于多项式)

何时重采样：每步都重采样 = 信息损失大。常用自适应重采样 — 只在有效粒子数 $N_{\text{eff}}$ 小于阈值时重采样：

N_{\text{eff}} = \frac{1}{\sum_i (w_i)^2}, \quad N_{\text{eff}} < N/2 \Rightarrow \text{resample}

6. 粒子退化与贫化

6.1 退化 (Degeneracy)

经过几个时间步后，几乎所有权重集中到一两个粒子上 — 等效粒子数极少，方差大。

根源：状态空间高维 + 观测信息丰富 → 大多数粒子无法匹配观测。

对策：

用更好的提议分布 (含观测信息 — Unscented PF, Extended PF)
频繁重采样

6.2 贫化 (Impoverishment)

重采样太频繁的副作用 — 大权重粒子被复制 N 次，粒子多样性消失，最终所有粒子都聚集在一点。

对策：

正则化粒子滤波 — 重采样时给每个粒子加微小高斯扰动
MCMC step — 重采样后用 MH 算法变异
辅助粒子滤波 (Auxiliary PF) — 改提议分布
粒子数自适应

7. SLAM 中的应用

7.1 MCL (Monte Carlo Localization)

(Fox, Burgard, Dellaert, Thrun 1999)

已知地图 + 激光/里程计 → 用 PF 在地图中定位
初始化时全图均匀撒粒子 → 经过几次观测后聚到正确位姿
经典使用场景：ROS amcl (Adaptive Monte Carlo Localization)

7.2 FastSLAM

(Montemerlo, Thrun, Koller, Wegbreit 2002)

Rao-Blackwellized 分解：

p(x_{0:t}, m | z_{1:t}, u_{1:t}) = p(x_{0:t} | z_{1:t}, u_{1:t}) \cdot p(m | x_{0:t}, z_{1:t})

路径用 PF 表示 (多模态、高度非线性)
每个粒子独立维护一个地图 (每个地图特征是个 EKF)
因为每个特征条件独立于路径，地图部分能解析处理

优点：把高维联合分布拆成低维条件 → 大幅降低所需粒子数与权重方差，远比在原始高维联合空间直接跑 PF 高效。

7.3 FastSLAM 2.0

(Montemerlo, Thrun, Koller, Wegbreit 2003)

改进提议分布 — FastSLAM 1.0 只用运动模型 (里程计) 作提议，观测精度高时粒子大量浪费；2.0 把当前观测也并入提议分布
提议更贴近真实后验 → 所需粒子数大幅减少，线性高斯情形下可证收敛
代价：每粒子提议计算更复杂 (需对观测做局部 EKF 式更新)

7.4 Gmapping

(Grisetti, Stachniss, Burgard 2007)

栅格地图 RBPF — 2D 激光 SLAM 的经典实现 (ROS gmapping)
扫描匹配引导的提议分布 — 用激光扫描匹配把提议收窄到观测峰值附近 (同 FastSLAM 2.0 思路)，再撒少量粒子
自适应重采样 — 按有效粒子数 $N_\text{eff}$ 触发，只在退化时重采样，缓解粒子贫化

7.5 缺点 (PF-SLAM 共性)

长路径 → 每个粒子的"路径采样"差异巨大，PF 难追踪
现代 SLAM 系统 (ORB-SLAM， VINS) 用因子图 + LM 替代了 PF/EKF
PF 在离散搜索/识别问题 (重定位、地点识别) 仍有市场

8. 变体

变体	改进
Extended PF (EPF)	用 EKF 估每个粒子的局部高斯近似作提议分布
Unscented PF (UPF)	用 UKF 作提议分布，避免 EKF 雅可比线性化误差
Auxiliary PF (APF)	重新设计提议分布让粒子提前对齐观测
Rao-Blackwellized PF	解析处理部分高斯子空间 (FastSLAM 的核心思想)
Box PF	用 interval 而非点表示粒子，鲁棒于有界但未知误差
GMM PF	用高斯混合而非粒子表示分布，比 PF 紧凑

9. 速查对比

方法	适用分布	适用非线性	SLAM 现状
KF	高斯	线性	线性子系统 (IMU 预积分协方差递推)
EKF	高斯	弱非线性	MSCKF, ROVIO
UKF / iEKF	高斯	中等非线性	部分新 LIO 系统
Particle Filter	任意	任意	MCL 定位，重定位
Rao-Blackwellized PF	高斯 + 任意混合	任意	FastSLAM (2D 激光 SLAM)
因子图 + LM	等效高斯 (MAP)	任意	ORB-SLAM, VINS, GTSAM — 现代主流

References

Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics. MIT Press, 2005. — 第 4 章 (Bayes Filters)，第 8 章 (Monte Carlo Localization)，第 13 章 (FastSLAM) — PF 在 SLAM 应用的标准教材
Doucet, A., Godsill, S., & Andrieu, C. (2000). On Sequential Monte Carlo Sampling Methods for Bayesian Filtering. Statistics and Computing. — SIS / SIR 理论奠基
Arulampalam, M. S., Maskell, S., Gordon, N., & Clapp, T. (2002). A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. IEEE T-SP. — PF 入门 tutorial 经典
Fox, D., Burgard, W., Dellaert, F., & Thrun, S. (1999). Monte Carlo Localization: Efficient Position Estimation for Mobile Robots. AAAI. — MCL 主要原始论文 (最常被引)
Dellaert, F., Fox, D., Burgard, W., & Thrun, S. (1999). Monte Carlo Localization for Mobile Robots. ICRA. — MCL 同期 ICRA 版本
Montemerlo, M., Thrun, S., Koller, D., & Wegbreit, B. (2002). FastSLAM: A Factored Solution to the SLAM Problem. AAAI
Montemerlo, M., Thrun, S., Koller, D., & Wegbreit, B. (2003). FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges. IJCAI. — 观测并入提议分布，粒子数大减
Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters. IEEE T-RO. — Gmapping，2D 激光栅格 RBPF
Barfoot, T. D. State Estimation for Robotics. Cambridge University Press, 2017. — 第 4 章 (Nonlinear Filtering)
高翔. 视觉 SLAM 十四讲 (第 2 版). 第 9 章 (后端 1) 与第 10 章 (后端 2) 间接涉及 PF 的工程优化替代方案

粒子滤波 — SIS， SIR, Rao-Blackwellized PF

Table of Contents