粒子滤波（PF）算法

1. 动机：当卡尔曼类方法不再够用

三维定位中，前面讨论的 LS/WLS/NLS/WNLS 本质上都是批量式估计：把当前时刻所有测距值堆在一起，解一次坐标。但真实系统往往是序贯的——目标在运动，每一时刻都有新的观测到达，而且运动模型和观测模型都可能带有非高斯噪声。

卡尔曼滤波（KF）在线性高斯条件下是最优的，但它要求模型线性、噪声高斯。扩展卡尔曼滤波（EKF）通过一阶泰勒展开处理非线性，代价是线性化误差，并且仍然假设后验分布是高斯的。无迹卡尔曼滤波（UKF）用 sigma 点避免了雅可比矩阵，精度更高，但仍然以高斯分布作为后验的近似形式。

粒子滤波走的是另一条路：不对后验分布的形状做任何高斯假设，而是用一组带权重的随机样本（粒子）直接“画出”后验分布。当粒子数足够大时，这组粒子可以逼近任意形状的后验——多峰、偏斜、重尾，都不需要额外的解析形式。

代价也很明确：计算量随粒子数线性增长，在高维状态空间中尤其昂贵。PF 的精度优势是用算力换来的。

---

2. 概率框架：递归贝叶斯估计

PF 求解的是递归贝叶斯估计中的积分问题。状态空间模型为：

$$\mathbf{x}_k = f(\mathbf{x}_{k-1}) + \mathbf{w}_k$$ $$\mathbf{y}_k = h(\mathbf{x}_k) + \mathbf{v}_k$$

其中 $\mathbf{w}_k$ 和 $\mathbf{v}_k$ 分别是过程噪声和观测噪声，不要求服从高斯分布。

递归贝叶斯估计分为两步：

预测：

$$p(\mathbf{x}_k \mid \mathbf{y}_{1:k-1}) = \int p(\mathbf{x}_k \mid \mathbf{x}_{k-1}) p(\mathbf{x}_{k-1} \mid \mathbf{y}_{1:k-1}) \,d\mathbf{x}_{k-1}$$

更新：

$$p(\mathbf{x}_k \mid \mathbf{y}_{1:k}) = \frac{ p(\mathbf{y}_k \mid \mathbf{x}_k)\, p(\mathbf{x}_k \mid \mathbf{y}_{1:k-1}) }{ p(\mathbf{y}_k \mid \mathbf{y}_{1:k-1}) }$$

理论上，只要知道后验 $p(\mathbf{x}k \mid \mathbf{y}{1:k})$，就可以计算最小均方误差估计：

$$\hat{\mathbf{x}}_k = \mathbb{E}[\mathbf{x}_k \mid \mathbf{y}_{1:k}] = \int \mathbf{x}_k\, p(\mathbf{x}_k \mid \mathbf{y}_{1:k}) \,d\mathbf{x}_k$$

问题在于：这个积分在高维、非线性、非高斯情况下通常没有解析解。PF 的出发点就是用蒙特卡罗采样代替积分。

---

3. 核心机制一：重要性采样

如果能够直接从后验 $p(\mathbf{x})$ 中采样，那么期望可以用样本均值近似。但后验通常无法直接采样。重要性采样引入一个容易采样的提议分布 $q(\mathbf{x})$，从 $q$ 中采样，再用权重修正偏差。

从未归一化权重：

$$\omega^{(i)} = \frac{p'(\mathbf{x}^{(i)})}{q(\mathbf{x}^{(i)})}$$

归一化后：

$$\tilde{\omega}^{(i)} = \frac{\omega^{(i)}}{\sum_{j=1}^{N}\omega^{(j)}}$$

目标分布的粒子近似为：

$$\hat{p}(\mathbf{x}) \approx \sum_{i=1}^{N} \tilde{\omega}^{(i)} \delta(\mathbf{x}-\mathbf{x}^{(i)})$$

任意函数的期望可以用加权和估计：

$$\mathbb{E}[g(\mathbf{x})] \approx \sum_{i=1}^{N} \tilde{\omega}^{(i)} g(\mathbf{x}^{(i)})$$

关键点：粒子本身只是“位置”，权重才是“可信度”。后验的形状由粒子位置和权重的联合分布共同刻画，而不是由粒子的核密度单独决定。

---

4. 核心机制二：序贯重要性采样（SIS）与重采样

4.1 SIS 的权重递推

将重要性采样序贯化，提议分布取为：

$$q(\mathbf{x}_{1:k} \mid \mathbf{y}_{1:k}) = q(\mathbf{x}_{1:k-1} \mid \mathbf{y}_{1:k-1}) \, q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{y}_k)$$

权重递推为：

$$\omega_k^{(i)} \propto \omega_{k-1}^{(i)} \, \frac{ p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)})\, p(\mathbf{x}_k^{(i)} \mid \mathbf{x}_{k-1}^{(i)}) }{ q(\mathbf{x}_k^{(i)} \mid \mathbf{x}_{k-1}^{(i)}, \mathbf{y}_k) }$$

最常用的选择是自举滤波（Bootstrap Filter） ：令提议分布等于状态转移先验，

$$q(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{y}_k) = p(\mathbf{x}_k \mid \mathbf{x}_{k-1})$$

此时权重递推简化为：

$$\omega_k^{(i)} \propto \omega_{k-1}^{(i)} \, p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)})$$

也就是说，新权重大小只取决于新观测对粒子的“支持程度”。粒子越能解释当前观测，权重越大。

4.2 粒子退化：SIS 的致命缺陷

如果不做任何处理，SIS 运行若干步后会出现权重退化：绝大多数粒子的权重趋近于零，只有极少数粒子承载几乎全部权重。此时大量计算被浪费在几乎无贡献的粒子上，有效粒子数急剧下降。

衡量退化程度的指标是有效样本数（Effective Sample Size, ESS） ：

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

通常设定阈值 $N{\text{th}}$（常用 $2N/3$），当 $N{\text{eff}} < N_{\text{th}}$ 时触发重采样。

4.3 重采样：把计算资源集中到“有希望”的粒子上

重采样的操作是：从当前带权粒子集中，按权重比例有放回地重新抽取 $N$ 个粒子，并将所有权重重置为 $1/N$。权重大的粒子被复制多次，权重小的粒子被淘汰。

重采样解决了权重退化，但引入了新的问题：粒子贫化（sample impoverishment） 。因为高权重粒子被重复复制，粒子集中出现大量完全相同的样本，多样性下降。如果过程噪声很小，粒子集可能在几步之内塌缩到少数几个点上。

退化与贫化是一对矛盾：不重采样，权重退化；重采样，多样性丧失。PF 的绝大多数改进工作，本质上都是在“抑制退化”和“保持多样性”之间寻找更好的平衡。

---

5. 经典重采样方法

四种基本重采样算法在文献中被反复比较：

方法	核心思想	特点
多项式重采样	独立地从权重分布中抽取 $N$ 次	最简单，方差最大
分层重采样	将 $[0,1]$ 分成 $N$ 个等长区间，每个区间内独立抽一个点	方差低于多项式，实现稍复杂
系统重采样	只抽一个均匀随机数，然后等间隔取 $N$ 个点	实现最简单，方差低，工程中最常用
残差重采样	先按整数部分确定性复制，残差部分再随机采样	确定性成分最高，方差最小，但多样性损失也最严重

仿真比较的一般结论是：系统重采样和分层重采样的运行时间接近，且优于多项式与残差重采样；设定重采样阈值可以显著减小整体跟踪误差。在工程实现中，系统重采样通常是默认选择，因为它在“低方差”和“实现简单”之间取得了很好的平衡。

---

6. 标准粒子滤波算法（SIR）

综合 SIS 与重采样，得到最常用的序贯重要性重采样（Sequential Importance Resampling, SIR） 算法。单步流程如下：

输入：上一时刻粒子集 ${\mathbf{x}{k-1}^{(i)}, \omega{k-1}^{(i)}}_{i=1}^{N}$，当前观测 $\mathbf{y}_k$。

Step 1 预测（采样）
对每个粒子，从状态转移先验中采样：

$$\mathbf{x}_k^{(i)} \sim p(\mathbf{x}_k \mid \mathbf{x}_{k-1}^{(i)})$$

Step 2 更新权重
计算未归一化权重：

$$\omega_k^{(i)} = \omega_{k-1}^{(i)} \, p(\mathbf{y}_k \mid \mathbf{x}_k^{(i)})$$

归一化：

$$\tilde{\omega}_k^{(i)} = \frac{\omega_k^{(i)}}{\sum_{j=1}^{N}\omega_k^{(j)}}$$

Step 3 计算 ESS 并判断是否重采样

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

若 $N{\text{eff}} < N{\text{th}}$，执行重采样，得到等权粒子集 ${\mathbf{x}_k^{(i)}, 1/N}$；否则保留原权重。

Step 4 状态估计

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

这一套流程的直观含义是：用运动模型把粒子“推”到新位置，用观测告诉每个粒子“你有多可信”，然后根据可信度决定是否把资源重新集中到有希望的粒子上，最后用加权平均给出估计。

---

7. 退化与贫化的改进方向

标准 SIR 的两个核心缺陷——权重退化和粒子贫化——催生了 PF 的主要改进路线。

7.1 改进重要性密度：不只用先验，而是用观测“引导”采样

Bootstrap Filter 用先验作为提议分布，采样时完全没有利用当前观测 $\mathbf{y}_k$。如果观测信息很强，先验采样出的粒子可能大量落在似然很低的区域，导致权重退化加剧。

改进思路是构造一个更接近后验的提议分布，让采样阶段就“朝着观测支持的方向走”。代表性方法包括：

• EKPF（Extended Kalman Particle Filter） ：用 EKF 产生每个粒子的重要性密度，把当前观测纳入采样过程；
• UPF（Unscented Particle Filter） ：用 UKF 产生重要性密度，避免 EKF 的线性化误差，在强非线性场景下通常优于 EKPF；
• 基于 UKF 与优化组合策略的改进 PF：UKF 负责重要性密度，优化组合重采样负责保持多样性，同时针对退化和贫化两个问题。

这类方法的核心收益是用额外的计算换取更低的权值方差，但代价是每个粒子都需要跑一次 EKF/UKF 预测-更新，总计算量显著高于 Bootstrap Filter。

7.2 改进重采样：在“复制高权重”和“保持多样性”之间折中

• 自适应重采样：只在 ESS 低于阈值时重采样，避免每一步都强行重采样导致不必要的多样性损失；
• 优化组合重采样 / 动态分层权重优化：对不同权重区间的粒子采用差异化的处理策略，避免高权重粒子被过度复制、低权重粒子被一刀切淘汰；
• 正则化重采样：在重采样后的粒子上施加小扰动（如核密度平滑），把复制的粒子重新“打散”；
• 基于 MCMC 的移动步骤：重采样后对粒子做若干步 MCMC 转移，在不改变目标分布的前提下恢复多样性。

7.3 一个需要澄清的常见误解

“增加粒子数”经常被当作解决 PF 问题的万能药。它确实可以推迟退化与贫化的发生，但不能从根本上消除它们；而且粒子数增加会线性推高计算成本，在实时三维定位中往往不可接受。真正有效的改进必须作用在重要性密度或重采样机制上，而不是单纯堆粒子。

---

8. PF 与 EKF、UKF 的定位

三者的核心区别在于对后验分布的表示方式：

方法	后验表示	非线性处理	噪声假设	计算成本	典型适用场景
EKF	高斯（均值 + 协方差）	一阶泰勒线性化	高斯	低	弱非线性、实时性优先
UKF	高斯（sigma 点传播）	无迹变换，无需求雅可比	高斯	低–中	中度非线性、高斯噪声
PF	任意分布（粒子集）	无需线性化或高斯近似	非高斯也可	高（随粒子数线性增长）	强非线性、非高斯、多峰后验

关键结论可以概括为：

• PF 的精度优势是有条件的：在强非线性或非高斯噪声下，PF 的估计精度可以显著优于 EKF/UKF；但在近似线性高斯的场景中，PF 未必比 UKF 更好，却要付出高得多的计算代价。
• 高维状态空间是 PF 的软肋：粒子数需要随状态维数增长而大幅增加，否则粒子集无法有效覆盖后验。三维定位中如果状态只包含位置（3 维）或位置+速度（6 维），PF 仍然可行；但若状态维数继续升高，计算效率会迅速成为瓶颈。
• 实时性要求高的场景，EKF/UKF 往往仍是首选；PF 更适合离线分析、精度优先、或后验明显非高斯的场合。

---

9. 在三维定位中的角色

在三维定位与跟踪中，PF 的位置可以这样理解：

• 静态定位（单帧解算） ：PF 不是首选。LS/WLS/NLS 的一次性求解更直接、更快。PF 的价值只有在引入时间维度的运动模型之后才会充分体现。
• 动态跟踪：目标在运动，状态为位置（以及可选的速度、加速度），观测为距离、TDOA、AOA 等。PF 可以处理非高斯测距噪声（如 NLOS 引起的偏斜分布）、多峰后验（目标可能在多个候选位置之间不确定），以及机动运动模型（转弯、加速）带来的非线性。
• 三维水下/室内等复杂环境：已有工作将 PF 用于三维水下目标跟踪，并针对粒子退化引入 UKF 构造重要性密度、针对粒子贫化设计动态自适应分层重采样；仿真中改进 PF 相比标准 PF 的均方根误差显著降低。这类场景的共同特点是观测非线性强、噪声非高斯、异常值频繁，恰好落在 PF 的设计优势区间内。

工程选型的核心判断：

1. 如果只需要单帧坐标，且误差近似零均值，不要用 PF，LS/WLS/NLS 的性价比高得多；
2. 如果目标在运动，观测噪声明显非高斯（NLOS、多径、异常值），且算力允许，PF 是比 EKF/UKF 更自然的选择；
3. 如果选择 PF，优先考虑 UPF 或带自适应重采样的 SIR，而不是裸的 Bootstrap Filter；裸 SIR 在观测信息较强时退化会非常快；
4. 如果实时性紧张，先用 EKF/UKF 跑一个基线，再评估 PF 带来的精度增益是否值得额外的计算开销。

---

10. 总结

粒子滤波的核心思想可以用一句话概括：用带权重的粒子集代替解析分布，用采样代替积分，用重采样对抗权重退化。

它的能力边界同样清晰：

• 强项：任意形状的后验、非高斯噪声、强非线性、多峰不确定性；
• 代价：计算量随粒子数和状态维数增长，重采样引入的粒子贫化需要额外机制补偿，参数（粒子数、重采样阈值、提议分布）对性能影响显著。

从方法演进的角度看，PF 不是对 EKF/UKF 的“全面替代”，而是在高斯假设不再成立、线性化误差不可忽略时的一条必要备选路径。在三维定位项目中，它最适合承担的角色是动态、非高斯、精度优先的跟踪后端，而不是静态单帧解算的主力。