【How Kalman Fliters Works，卡尔曼滤波（二）粒子滤波器】

​
本文翻译自本文翻译自How Kalman Filters Work，考虑到专业水平有限，如有问题和建议，欢迎在评论区讨论！

粒子滤波器非常简单，但它的形式和要解决的问题一样多。这里介绍一种简单的粒子滤波器——bootstrap 滤波器，以了解其基本原理。

粒子滤波器的一次迭代

  粒子过滤器是这样工作的：首先，我们会问：“可能会发生哪些事情，每件事的可能性有多大？”然后，“对于可能发生的每件事，我希望观察到什么？”然后，” 根据我现在实际观察到的情况，每件事的可能性有多大？”然后我们再重复一遍。

  假设我们大致知道一个弹跳球的原始位置，不知道它的速度。让我们创造一堆” 假想”球，它们随机分布在我们所认为球的实际位置（测量值）的周围，速度也是随机的。初始时刻，这些“假想”球的状态量是“真”球状态量的概率相同。
​

图1. 小球初始位置和速度

  这里的一堆“假想”球就好比引言中的一系列国家。每个“假想”球的状态称为一个粒子，粒子代表真实状态的可能性就是粒子权重。因此，对于每个粒子，我们都有一个“假想”球的位置和速度，以及该球描述真实状态的可能（从 0 到 1），其中 1 代表完全确定，0 代表完全不可能。例如，南乔治亚岛的粒子权重正好为 0，因为南乔治亚岛上没有寿司店（尽管那里有很多鱼）。过了一个时间间隔之后，我们测量出球的位置。

图2. 新的测量

  球是如何到达这个位置的？它是先上升，然后慢慢划弧再下降的吗？还是先往下走，经过地面反弹，然后再上来的吗？我们还不知道，两种路径的可能性都是一样的。我们还知道，测量结果并不完全准确；它必定存在一些噪音。因此，让我们将每个“假想”球都向前传播到测量时刻，看看它对实际测量结果的预测有多准确。用${\hat{\mathbf{x}}}_{i,k-1}$表示第 i个粒子在上一采样时刻的状态（两个位置量和两个速度量），用${\hat{\mathbf{x}}}_{i,k}$表示第 i 个粒子在当前采样时刻的状态。其中$\hat{\mathbf{x}}$表示估计值，$k$表示样本。弹跳球的动力学方程是已知的，因此可以通过传播函数$f$，根据上一时刻小球的状态推演得到小球当前时刻的状态。
$${\hat{\mathbf{x}}}_{i,k}=f({\hat{\mathbf{x}}}_{i,k-1})$$ 通过这个传播函数来更新每个粒子的位置和速度：从上一时刻的位置和速度开始，计算小球在重力作用下划出的抛物线、何时/是否与地板相交、从地板向上返回的轨迹等，直到推演至当前时刻。可以说，传播函数是一个小仿真。根据更新后的位置，我们利用已知的观测函数$h$来预测每个粒子的测量结果：
$${\hat{\mathbf{z}}}_{i,k}=h({\hat{\mathbf{x}}}_{i,k})$$其中$\hat{\mathbf{x}}$表示预测测量值，以帮助我们将其与实际测量值${\mathbf{z}}_k$区分开来。在这个问题中，观测函数只是返回小球的位置。

  从下图可以看出，有些粒子能很好地预测测量结果，而有些粒子则很差。

图3. 粒子的传播

  我们可以利用每个球的新位置与测量值之间的差$(z_k-{\hat{z}}_{i,k})$，然后说："考虑到我们所知道的测量值的噪声程度，出现这么大的测量误差的几率有多大？真正糟糕的预测几率很低，而好的预测几率会很高。

图3. 预测测量误差

  这样，我们就有了每个“假想”球的测量误差概率。我们可以用每个粒子的之前的权重乘以这个概率得到新的权重。结果是，最能预测测量结果的粒子最终权重最高。下图，根据新的权重对轨迹进行着色来说明这一点：（注意，做完这个乘法后，权重之和不等于1，需要重新进行归一化处理）

图3. 根据最新权重对轨迹进行着色

  利用这些权重，我们可以计算出所有“假想”球的加权平均值。这可能是对状态的最佳估计（我们还可以使用许多其他方法来计算最佳估计的概率，其中加权平均是最简单的方法）。

图3. 加权平均值作为状态估计

  此时，我们就有了一个更新的\textbf{状态估计值}和相关的\textbf{不确定性}，这里的不确定性表现为一个有限的粒子集合。现在，我们等待另一次测量结果的到来，并再次运行这一过程。我们将每个球传播到测量时刻，计算测量与“假想”球之间的误差概率，并更新该球的概率。所以，让我们再进行一次循环。

第二次迭代，加入重采样（resampling）和正则化（regularization）

传播.

图3. 新的测量结果和传播后的粒子

  有些粒子能很好地预测新的测量结果，有些粒子则严重偏离测量结果。更新每个粒子的概率。

图3. 再次更新权重

  请注意，此时我们最初的大部分“假想”球现在已经没用了（大部分线条的颜色已经很淡）。它们根本无法代表实际运动状态。另外，它们都开始分散得很远。最终，它们都会发散，我们的滤波器就会失效。粒子过滤器之所以能发挥作用，关键在于粒子集每隔一段时间（甚至每次更新后）就会重新计算一次，就像我们从可能的全球地点列表切换到更精细的日本地点列表一样。有很多方法可以做到这一点，但这里有一个简单的方法：

1. 根据粒子权重从当前粒子集中随机生成粒子，创建新的粒子集。因此，一般来说，权重为 0.04 的粒子出现的频率是权重为 0.02 的粒子的两倍。
2. 创建新粒子集后，将所有权重设回 1/N。现在，不确定性的分散性是通过不同粒子的存在来体现的，而不是通过粒子权重来体现。需要说明的是：此时会有很多重复的粒子。
3. 计算新粒子集的样本协方差，或其他衡量粒子离散程度的指标。样本协方差的计算特别简单：
   $$P_k=\frac{1}{n-1}\sum _{i=1}^n({\hat{\mathbf{x}}}_{i,k}-\overline{\mathbf{x}})({\hat{\mathbf{x}}}_{i,k}-\overline{\mathbf{x}}{)}^T$$
4. 从样本协方差矩阵中进行高斯随机抽取（见附录）。将这些抽样乘以一些小的调整参数，得到一组扰动。给每个粒子添加一个扰动。

这样就会产生一组新粒子，散布在状态空间的高概率区域。前两个步骤称为重采样，最后两个步骤称为正则化粒子。让我们对当前的粒子集进行正则化：

图3. 对粒子进行重采样和正则化

Bootstrap滤波器的框架

  我们接下来可以进行下一次测量，每一步的流程可以总结成以下6条：

1. 传播粒子
2. 根据新的测量结果，确定每个粒子的可能性有多大
3. 更新权重（并归一化以保证所有粒子的权重之和为1）
4. 计算状态估计值（e.g.加权平均值）
5. 如果这一步对粒子重新采样，则根据粒子的权重随机生成粒子，然后将权重恢复为 1/N
6. 如果这次要对粒子进行正则化处理，则要对新的粒子集施加小规模的随机扰动。

  回到弹跳球的例子，下面是仿真10s的结果，不确定性明显集中在真实状态（蓝色）周围。

图3. 仿真10s的结果

假设、优势、劣势

粒子滤波器适用的两点假设：

• 可以近似描述状态如何随时间变化($f$)
• 可以大致确定假设测量误差的可能性。

粒子滤波的优势非常明显：只需很少的理论工作，就能对状态进行粗略估计，而且适用于许多不同类型的问题。
不确定性（粒子集）甚至可以分布在状态空间的多个区域，代表多模态概率分布。此外，粒子滤波过程可根据不同问题轻松定制（例如，何时以及如何重新采样、如何评估测量误差、如何确定初始粒子）。
这使得粒子滤波具有灵活性和广泛的实用性。

劣势也非常明显：

• 粒子滤波器通常需要大量粒子，这可能会耗费大量运行时间。即使是最简单的粒子过滤器，也需要使用 1000 个粒子，每次测量需要 1000 次模拟。随着状态维度的增加，所需的粒子数量也会变得巨大。
• 将不确定性表示为一组粒子和权重（一种离散的概率分布），意味着对状态的最佳估计通常非常粗糙，因此粒子滤波器在处理需要高精度的问题时效果不佳。
• 当需要更好的性能时，通常必须对粒子滤波器进行大幅调整，以适应每个单独的状态估计问题，而这可能需要很长时间，尤其是因为测试需要运行滤波器，这本身就需要很长时间。出于同样的原因，我们很难找到一种有用的通用粒子滤波器，尽管bootstrap滤波器在处理简单问题时还不错。

  粒子滤波器是最近才开始流行起来的，这是因为现在的处理能力和分布式计算非常廉价且易于使用。此外，粒子滤波器还有许多新技术，可以缩短运行时间并提高精度。然而，对于需要速度的应用，我们通常需要一种不同的过滤架构。