粒子滤波器

粒子滤波（Particle Filter）通过一组带权重的随机粒子近似状态的后验分布，适用于非线性非高斯系统的状态估计，是 EKF 之外的另一大类滤波方法。

以粒子之众，逼近状态之真

为什么需要粒子滤波

卡尔曼滤波（KF）在线性高斯假设下能得到解析的最优解，扩展卡尔曼滤波（EKF）通过一阶泰勒展开将非线性问题局部线性化。但现实中许多系统的运动模型与观测模型是强非线性的，噪声也往往不服从高斯分布，此时线性化会产生较大误差，甚至导致滤波器发散。

粒子滤波的思路是：既然无法用解析表达式描述后验分布，就用一组带权重的随机样本（粒子）来近似它。只要粒子足够多，采样近似就可以任意逼近真实的后验分布。

贝叶斯滤波框架

状态估计可以统一在贝叶斯滤波框架下。设系统状态为 $x_k$，观测为 $z_k$，则递推估计分为两步：

1. 预测：利用运动模型 $p(x_k|x_{k-1})$ 由上一时刻的后验 $p(x_{k-1}|z_{1:k-1})$ 推得先验：

   $$p(x_k|z_{1:k-1}) = \int p(x_k|x_{k-1}) p(x_{k-1}|z_{1:k-1}) dx_{k-1}$$

2. 更新：利用观测模型 $p(z_k|x_k)$ 修正先验得到后验：

   $$p(x_k|z_{1:k}) \propto p(z_k|x_k) p(x_k|z_{1:k-1})$$

KF 在特定假设下有闭式解，而粒子滤波用数值采样的方式同时处理预测与更新两步。

重要性采样与序贯重要性采样（SIS）

直接从后验 $p(x_k|z_{1:k})$ 采样通常是困难的。重要性采样引入一个容易采样的建议分布 $q(x)$，用粒子及其权重近似后验：

$$w_k^{(i)} \propto \frac{p(x_k^{(i)}|z_{1:k})}{q(x_k^{(i)})}$$

结合递推结构，可导出序贯重要性采样（Sequential Importance Sampling, SIS）的权重递推公式：

$$w_k^{(i)} \propto w_{k-1}^{(i)} \frac{p(z_k|x_k^{(i)}) p(x_k^{(i)}|x_{k-1}^{(i)})}{q(x_k^{(i)}|x_{k-1}^{(i)}, z_k)}$$

即每来一个新观测，只需利用运动模型与观测模型更新粒子权重，而无需重新采样所有历史粒子。

粒子退化与重采样

SIS 存在一个严重问题——粒子退化：经过若干次递推后，大部分粒子的权重趋近于 0，只剩少数粒子携带有效信息，浪费大量计算资源。

衡量退化程度的指标是有效粒子数 $N_{eff} = 1 / \sum_i (w^{(i)})^2$。当 $N_{eff}$ 低于阈值时，需要进行重采样：按照权重大小从旧粒子集中重新抽取 $N$ 个粒子，权重大的粒子被复制、权重小的被淘汰，之后所有粒子权重重新设为 $1/N$。

常用重采样方法包括：多项式重采样、系统重采样、残差重采样等。重采样以损失粒子多样性为代价换取粒子有效性，因此实际中常结合二者（如 Sampling Importance Resampling）。

标准粒子滤波算法流程

1. 初始化：从先验分布 $p(x_0)$ 采样 $N$ 个粒子 $\{x_0^{(i)}\}_{i=1}^N$，权重均设为 $1/N$。
2. 预测：按运动模型 $x_k^{(i)} \sim p(x_k|x_{k-1}^{(i)})$ 递推每个粒子。
3. 更新：按观测似然更新权重 $w_k^{(i)} = w_{k-1}^{(i)} p(z_k|x_k^{(i)})$，并归一化。
4. 重采样：若 $N_{eff} < N_{th}$，进行重采样。
5. 输出估计：$\hat{x}_k = \sum_i w_k^{(i)} x_k^{(i)}$，回到第 2 步循环。

与其他滤波方法的对比

应用

• SLAM：如 FastSLAM 用粒子滤波估计机器人位姿，每个粒子维护一份地图。
• 目标跟踪：非线性运动与量测场景下的多目标跟踪。
• 定位：结合 GPS/IMU 的非线性融合定位。
• 机器人状态估计：非高斯噪声环境下的位姿与速度估计。

小结

粒子滤波以”样本近似分布”的蒙特卡洛思想，突破了 KF 系列的线性高斯局限，代价是计算开销与粒子退化问题。理解贝叶斯递推框架是掌握粒子滤波的关键，重采样则是保证算法长期稳定运行的灵魂。