粒子滤波（PF）算法（通俗版）

1. 一句话理解粒子滤波

粒子滤波就是“撒一把点，看哪个点最像真的，然后围着像的点再撒”。

想象你在一个黑屋子里找一个隐藏的物体，你手里有一堆测距仪（AP），但测距有误差。你不知道物体在哪，于是：

1. 先随便撒 2000 个“猜测点”，均匀铺满整个房间——这就是粒子；
2. 每个猜测点都拿去和测距仪比对：如果这个点算出来的距离和测距仪测到的差不多，说明这个点“靠谱”，给它高分；差很多，就给低分；
3. 把低分的点扔掉，围着高分点重新撒点——这样下一轮猜测就更集中、更接近真实位置；
4. 反复迭代，点越来越集中，最后取加权平均就是定位结果。

这就是粒子滤波的全部核心逻辑。下面把每一部分拆开讲清楚。

---

2. 为什么需要粒子滤波？它解决什么问题

2.1 前面方法的问题

你之前学的 LS/WLS/NLS/WNLS，都是“一次性算一个坐标”：

• 把当前所有测距值堆在一起，解一次方程，得到一个坐标；
• 解完就完了，不记得上一时刻在哪。

但真实场景里，目标在动，而且：

• 测距噪声不是高斯分布（NLOS 会让测距偏大，分布是偏斜的）；
• 目标可能在多个候选位置之间不确定（后验是多峰的）；
• 运动模型可能是转弯、加速，非线性很强。

卡尔曼滤波（KF/EKF/UKF）要求后验是高斯分布，遇到多峰、偏斜就无能为力。粒子滤波不要求后验是高斯——它用一堆点直接“画”出任意形状的后验。

2.2 一个直观对比

方法	怎么表示“目标在哪”	能处理多峰吗	能处理非高斯吗
EKF/UKF	一个均值 + 一个协方差（高斯钟形曲线）	不能	不能
粒子滤波	2000 个带分数的点	能	能

代价是：粒子滤波计算量大，2000 个点每个都要算一遍，比解一次方程慢得多。

---

3. 核心概念：粒子、权重、重采样

3.1 粒子是什么

粒子 = 一个“猜测的位置”。

比如三维场景里，一个粒子就是：

$$\mathbf{x}^{(i)} = (x^{(i)}, y^{(i)}, z^{(i)})$$

2000 个粒子就是 2000 个猜测位置。初始时不知道目标在哪，就均匀撒满整个场景——每个位置都有可能，谁也不比谁更可信。

3.2 权重是什么

权重 = 这个猜测有多可信。

每个粒子有一个权重 $\omega^{(i)}$，初始都是 $1/2000$（平等）。

当测距数据来了之后：

• 如果某个粒子算出来的“理论距离”和 AP 实测距离很接近，说明这个粒子靠谱，权重变大；
• 如果差很远，说明这个粒子不靠谱，权重变小。

权重更新公式（高斯似然）：

$$\omega^{(i)} \leftarrow \omega^{(i)} \times \prod_{j} \exp\left(-\frac{(d_j - \|\mathbf{x}^{(i)}-\mathbf{a}_j\|)^2}{2\sigma^2}\right)$$

看不懂公式没关系，记住一句话就行：

粒子算出的距离和实测距离差得越小，权重越大。

3.3 重采样是什么

重采样 = 扔掉不靠谱的粒子，复制靠谱的粒子。

跑了几轮之后，会出现一个问题：少数几个粒子权重特别大，其他几千个粒子权重几乎为零。这叫权重退化——相当于 2000 个点里只有 5 个在干活，其他 1995 个都在浪费计算资源。

重采样的做法：

• 按照权重比例，有放回地抽 2000 次；
• 权重大的粒子被抽中多次（复制多份）；
• 权重小的粒子几乎抽不到（被淘汰）；
• 抽完之后，所有粒子的权重重新设为相等（$1/2000$）。

这样计算资源就集中到了“有希望”的粒子上。

3.4 重采样带来的新问题：粒子贫化

重采样把高权重粒子复制了很多份，导致粒子集中出现大量完全相同的位置——多样性下降了。这叫粒子贫化。

如果过程噪声很小，粒子集可能在几步之内塌缩到同一个点，再也探索不到其他区域。

解决办法：重采样后给每个粒子加一点随机微扰（小噪声），把它们重新“打散”一点。

一句话总结：不重采样，权重退化；重采样，多样性丧失。粒子滤波的改进都是在这两者之间找平衡。

---

4. 标准粒子滤波的完整流程（SIR）

把上面的概念串起来，就是最常用的序贯重要性重采样（SIR） 算法。每一步都很简单：

输入

上一时刻的粒子集：2000 个位置 + 2000 个权重；当前时刻的测距数据。

Step 1：预测——把粒子“推”到新位置

目标在动，粒子也要跟着动。根据运动模型（匀速、加速、随机游走等），把每个粒子从上一时刻的位置推到当前时刻：

$$\mathbf{x}_k^{(i)} = \mathbf{x}_{k-1}^{(i)} + \text{运动带来的位移} + \text{随机噪声}$$

直观理解：目标往前走了 1 米，粒子也各自往前挪 1 米左右，但每个粒子挪的量略有不同（加了随机噪声）。

如果粒子被推到了场景外面，要处理一下：

• 裁剪：直接拉回边界；
• 反弹：像撞墙一样弹回来；
• 中间扩散：往场景中心方向拉。

Step 2：更新权重——用测距数据打分

对每个粒子，计算它到每个 AP 的“理论距离”，和实测距离比较：

$$\text{残差} = \text{实测距离} - \text{理论距离}$$

残差越小，权重越大：

$$\omega^{(i)} \leftarrow \omega^{(i)} \times \text{高斯似然}(\text{残差})$$

然后归一化，让所有权重加起来等于 1。

直观理解：测距仪说“目标离我 5 米”，某个粒子算出来“目标离我 4.9 米”，残差 0.1 米，这个粒子很靠谱，加分；另一个粒子算出来“离我 8 米”，残差 3 米，不靠谱，减分。

Step 3：判断是否重采样

计算有效粒子数（ESS） ：

$$N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N}(\tilde{\omega}^{(i)})^2}$$

ESS 的直观含义：“相当于有多少个粒子在真正起作用”。

• 如果 2000 个粒子权重都差不多，ESS ≈ 2000，很好；
• 如果只有 5 个粒子权重大，其他都是零，ESS ≈ 5，很糟糕。

当 ESS 低于阈值（比如 $2N/3 \approx 1333$）时，触发系统重采样：

1. 生成一个随机起点；
2. 等间隔取 2000 个采样点；
3. 按累积权重找到每个采样点对应的粒子，复制它；
4. 权重重置为 $1/2000$；
5. 给每个粒子加小噪声（微扰），防止贫化。

Step 4：输出估计

加权平均所有粒子的位置：

$$\hat{\mathbf{x}}_k = \sum_{i=1}^{N} \tilde{\omega}_k^{(i)} \mathbf{x}_k^{(i)}$$

直观理解：权重大的粒子说话分量大，权重小的说话分量小，加权平均就是“大家投票决定目标在哪”。

---

5. 用一张图理解整个流程

初始：2000 个粒子均匀撒满场景，权重相等
         ↓
┌─────────────────────────────────┐
│  Step 1：预测                    │
│  每个粒子按运动模型往前推一步      │
│  （加随机噪声，处理边界）          │
└─────────────────────────────────┘
         ↓
┌─────────────────────────────────┐
│  Step 2：更新权重                 │
│  每个粒子算“理论距离”             │
│  和实测距离比较，残差小 → 权重大   │
└─────────────────────────────────┘
         ↓
┌─────────────────────────────────┐
│  Step 3：判断是否重采样            │
│  ESS < 阈值？                    │
│  是 → 系统重采样 + 加微扰          │
│  否 → 保留原权重                  │
└─────────────────────────────────┘
         ↓
┌─────────────────────────────────┐
│  Step 4：输出                     │
│  加权平均 = 当前定位结果           │
└─────────────────────────────────┘
         ↓
    下一时刻，回到 Step 1

---

6. 重采样的四种方法（简单对比）

重采样就是“按权重抽签”，但抽签方式有四种：

方法	怎么抽	特点
多项式重采样	独立抽 2000 次	最简单，但随机性最大
分层重采样	把 $[0,1]$ 分成 2000 段，每段抽一个	比多项式稳
系统重采样	只抽 1 个随机数，然后等间隔取 2000 个点	最常用：简单 + 稳定
残差重采样	先按整数部分确定复制，余数再随机	最确定，但多样性最差

工程上默认用系统重采样，因为它实现简单、方差低，效果足够好。

---

7. 粒子滤波的优缺点（一句话版）

优点

• 不要求高斯分布：后验是啥形状都能表示（多峰、偏斜、重尾）；
• 不要求线性模型：运动模型和观测模型多复杂都行；
• 天然处理多峰不确定性：目标可能在多个位置，粒子集会自然分成几簇。

缺点

• 计算量大：2000 个粒子每个都要算，比解一次方程慢得多；
• 需要调参：粒子数、重采样阈值、微扰大小都影响效果；
• 高维状态空间很吃力：三维位置（3 维）还行，如果状态加到 10 维以上，粒子数要指数级增加；
• 粒子贫化：重采样后多样性下降，需要额外机制补救。

---

8. 什么时候用粒子滤波？（工程选型）

不要用 PF 的场景

• 只需要单帧坐标（静态定位）：用 LS/WLS/NLS 更快更简单；
• 噪声近似高斯、模型近似线性：EKF/UKF 足够，PF 是杀鸡用牛刀；
• 实时性要求极高、算力有限：PF 的计算量可能扛不住。

适合用 PF 的场景

• 目标在运动，需要跟踪；
• 测距噪声明显非高斯（NLOS 偏斜、多径、异常值）；
• 后验多峰（目标可能在几个候选位置之间不确定）；
• 运动模型非线性强（转弯、加速、机动）；
• 算力允许（有 GPU 或足够 CPU 预算）。

一句话建议

静态单帧用 LS/WLS/NLS，动态跟踪 + 非高斯噪声用 PF。
如果选了 PF，优先用“系统重采样 + 自适应触发 + 微扰”的 SIR，不要用裸的 Bootstrap Filter。

---

9. 和你已有项目的对应关系

你之前描述的项目：

2000 粒子均匀分布 → 每 T 个时刻三阶段更新（预测 → 自然更新 → 系统重采样）→ 两阶段 AP 剔除

这就是标准 SIR 粒子滤波 + 两阶段 AP 剔除的组合：

你的项目	对应本文档
2000 粒子均匀撒场景	初始化（第 3.1 节）
四种运动模型自由裁剪/反弹/中间扩散	Step 1 预测（第 4 节）
高斯自然对每个 AP 残差算权重	Step 2 更新权重（第 4 节）
ESS 低于阈值触发系统重采样 + 微扰	Step 3 重采样（第 4 节）
两阶段 AP 剔除（8 米阈值，至少保留 3 个）	本文档未展开，属于观测预处理，在 Step 2 之前执行

而 WPF（加权粒子滤波） 的区别只在 Step 2：每个 AP 的似然贡献不再等权，而是按距离反比加权——远端 AP 自动降权，近端 AP 话语权更大。

---

10. 总结：记住这五句话就够了

1. 粒子滤波 = 撒点 → 打分 → 淘汰 → 再撒 → 加权平均；
2. 粒子是“猜测位置”，权重是“可信度”，重采样是“淘汰不靠谱的、复制靠谱的”；
3. 权重退化（少数粒子独大）靠重采样解决，粒子贫化（多样性丧失）靠微扰缓解；
4. 系统重采样是工程默认选择，简单且稳定；
5. 静态定位不要用 PF，动态跟踪 + 非高斯噪声才是 PF 的主场。