Skip to content

粒子滤波 (Particle Filter)#

粒子滤波是一种基于蒙特卡洛方法 (Monte Carlo Method) 的递归贝叶斯估计器。它的核心原理是通过在状态空间中撒布一组带有权重的随机样本（称为“粒子”），来近似表示系统状态的后验概率密度分布。

它突破了卡尔曼滤波中“线性”与“高斯分布”的严格假设，是处理强非线性、非高斯、多模态系统状态估计的有力工具。

算法思路#

粒子滤波的核心是通过离散粒子的集合来逼近连续的概率分布，基本流程包含五个步骤：

  1. 初始化 (Initialize)：在初始状态空间按先验分布随机采样，生成初始粒子群。
  2. 预测步骤 (Predict)：将每个粒子代入系统的状态方程向前递推（叠加过程噪声），模拟系统的物理演化。
  3. 更新步骤 (Update)：结合最新的观测数据，评估每个粒子所处状态的“合理性”（即似然度），并据此更新该粒子的权重。
  4. 重采样 (Resample)：淘汰权重极低的粒子，复制权重高的粒子，防止计算资源浪费在无效区域（解决粒子退化问题）。
  5. 状态估计 (Estimate)：对所有粒子的状态进行加权平均，输出最终的最优估计值。

采样重要性重采样 (SIR) 粒子滤波#

SIR (Sampling Importance Resampling) 是最标准、最常用的粒子滤波算法框架。

数学公式#

1. 概率表示#

用 \(N\) 个粒子及其对应权重来近似 \(k\) 时刻的后验概率分布：

\[p(x_{0:k} | z_{1:k}) \approx \sum_{i=1}^{N} w_k^{(i)} \delta(x_{0:k} - x_{0:k}^{(i)})\]

其中 \(w_k^{(i)}\) 是第 \(i\) 个粒子的归一化权重，\(x_k^{(i)}\) 是对应的粒子状态，\(\delta(\cdot)\) 为狄拉克函数。

2. 预测步骤 (状态转移)#

从重要性分布（通常直接取系统的先验状态转移分布）中采样生成下一时刻的粒子：

\[x_k^{(i)} \sim p(x_k | x_{k-1}^{(i)}, u_k)\]

示例：对于最简单的加性随机游走模型 \(x_k = x_{k-1} + w_k\)：

\[x_k^{(i)} = x_{k-1}^{(i)} + w_k, \quad w_k \sim \mathcal{N}(0, Q)\]

3. 更新步骤 (权重计算)#

根据当前时刻的实际观测值 \(z_k\)，计算每个粒子的观测似然，并更新权重：

\[w_k^{(i)} \propto w_{k-1}^{(i)} \cdot p(z_k | x_k^{(i)})\]

示例：若观测模型为 \(z_k = h(x_k) + v_k\)，且测量噪声 \(v_k \sim \mathcal{N}(0, R)\)，则似然概率为高斯概率密度：

\[p(z_k | x_k^{(i)}) = \frac{1}{\sqrt{|2\pi R|}} \exp\left(-\frac{1}{2} (z_k - h(x_k^{(i)}))^T R^{-1} (z_k - h(x_k^{(i)}))\right)\]

最后必须对权重进行归一化，使其总和为 1：

\[w_k^{(i)} = \frac{w_k^{(i)}}{\sum_{j=1}^{N} w_k^{(j)}}\]

4. 重采样步骤#

重采样的目的是为了避免粒子退化 (Particle Degeneracy)。通常通过计算有效粒子数 \(N_{eff}\) 来决定是否触发重采样：

\[N_{eff} = \frac{1}{\sum_{i=1}^N (w_k^{(i)})^2}\]

若 \(N_{eff}\) 小于设定的阈值（例如 \(N/2\)），则执行重采样。常用的高效算法为系统重采样 (Systematic Resampling)：

import numpy as np

def systematic_resample(weights):
    N = len(weights)
    # 计算权重的累积分布函数 (CDF)
    cumsum = np.cumsum(weights)
    cumsum[-1] = 1.0  # 防止浮点误差

    step = 1.0 / N
    start = np.random.uniform(0, step)
    samples = start + np.arange(N) * step

    indices = np.zeros(N, dtype=int)
    j = 0
    for i in range(N):
        while cumsum[j] < samples[i]:
            j += 1
        indices[i] = j

    return indices # 返回被选中的粒子索引

5. 状态估计#

在完成权重更新（或重采样）后，系统的最优状态估计通常取所有粒子的加权期望：

\[\hat{x}_k = \sum_{i=1}^{N} w_k^{(i)} x_k^{(i)}\]

参数说明与调优指南#

参数 含义 调整建议
\(N\) 粒子数量 通常在 100~10000 之间。状态维度越高，需要的 \(N\) 呈指数增长。
\(Q\) 过程噪声方差 决定粒子在预测步的“发散程度”。\(Q\) 越大，粒子覆盖面越广，跟踪突变能力越强，但稳态时估计波动大。
\(R\) 测量噪声方差 决定权重更新时的“宽容度”。\(R\) 越大，不同粒子的权重差异越小；\(R\) 过小可能导致只有极少数粒子获得权重，加剧粒子退化。
\(N_{eff}\) 有效粒子数阈值 通常设为 \(N/2\) 或 \(2N/3\)。过高会导致频繁重采样引发“粒子耗尽”，过低则无法解决“粒子退化”。

工程调参原则：

  • 跟踪严重滞后/跟丢：大概率是系统发生了未建模的突变，需增大 \(Q\) 以扩大搜索范围，或直接增加粒子数 \(N\)。
  • 估计结果噪声过大：说明粒子过于分散且轻信观测，需减小 \(Q\) 或增大 \(R\)。

控制流程 (伪代码)#

初始化: 
    根据先验分布随机生成 N 个粒子 x_0[i]
    初始权重 w_0[i] = 1 / N

循环 k=1, 2, ... :
    # ================= 1. 预测 (Predict) =================
    for i = 1 to N:
        x_k[i] ~ p(x_k | x_{k-1}[i], u_k)  # 添加过程噪声进行状态递推

    # ================= 2. 更新 (Update)  =================
    w_sum = 0
    for i = 1 to N:
        likelihood = p(z_k | x_k[i])       # 计算观测似然
        w_k[i] = w_{k-1}[i] * likelihood
        w_sum += w_k[i]

    for i = 1 to N:
        w_k[i] = w_k[i] / w_sum            # 权重归一化

    # ================= 3. 重采样 (Resample) ==============
    N_eff = 1 / sum(w_k[i]^2 for i in 1 to N)

    if N_eff < Threshold:
        x_k = Resample(x_k, w_k)           # 按照权重优胜劣汰复制粒子
        for i = 1 to N:
            w_k[i] = 1 / N                 # 重采样后权重全部重置为均等

    # ================= 4. 估计 (Estimate) ================
    x_hat_k = sum(w_k[i] * x_k[i] for i in 1 to N)

局限性#

  1. 粒子退化与耗尽 (Degeneracy & Impoverishment)：经过多次迭代，权重会集中在极少数粒子上（退化）。虽然重采样能淘汰低权重粒子，但会使得高权重粒子被大量复制，导致粒子群失去多样性（耗尽）。
  2. 计算复杂度极高：运算时间复杂度为 \(O(N \cdot d)\)（\(d\) 为状态维度），尤其在大样本量下，实时性远低于卡尔曼滤波。
  3. 维数灾难 (Curse of Dimensionality)：当状态空间的维度较高（如 >6 维）时，要保持相同的分布覆盖率，所需的粒子数量会呈指数级爆炸。
  4. 提议分布的次优性：标准 SIR 滤波直接使用先验概率作为提议分布，没有利用最新的观测信息 \(z_k\)。在一些改进算法（如 EKF-PF 或 UPF）中，会利用 EKF 或 UKF 为每个粒子生成更好的提议分布。

与卡尔曼滤波对比#

特性 卡尔曼滤波类 (KF / EKF / UKF) 粒子滤波 (PF)
系统模型要求 KF需严格线性，EKF/UKF可处理局部非线性 适应任意强非线性系统
噪声分布假设 严格要求高斯白噪声 适应任意概率分布 (高斯/非高斯均可)
后验概率形态 仅能维持单峰高斯分布 可维持多模态 (多峰) 分布
计算复杂度 \(O(n^3)\) (主要耗时在矩阵求逆) \(O(N \cdot d)\) (计算量受粒子数 \(N\) 主导)
最优性 满足线性高斯条件时是全局最优 随粒子数 \(N \to \infty\) 渐进逼近最优
适用场景 导航、目标跟踪 (单目标/清晰模型) 计算机视觉跟踪、全局重定位 (Kidnapped Robot)

参考实现#

  • 面向对象的底层代码实现：请参见 utils.py 中的 ParticleFilter 类定义。