Particle Filter Simulation
Particle Filter
I. Introduction
大二买了一本 Probability Robotics(of Sebastian Thrun, et al)，其中粒子滤波的概率论知识讲得很清楚。大二学完概率论之后给我看明白了，人生第一次在理论学习上那么有成就感。但是看完之后总觉得干巴巴的，不知道如何应用。最近写完了 Volume2D Shader，突然想到一个很好的方法可以应用粒子滤波，于是实现了一版。半天写完，效果不错，见[Github Repo🔗:Enigmatisms/ParticleFilter]：
II. 理论推导
2.1 目标
我仅仅讨论粒子滤波在localization中的作用，虽然localization只是PF理论应用的一个小方向。如果没有给定初值，也没有很好地利用观测信息，localization基本就是在乱猜其在空间中的位置。并且，通常情况下，只使用当前scan也无法准确获得定位，因为可能存在大量的重复地图特征。
好在我们有连续的观测以及连续的控制，假设控制与观测序列：
- 控制序列: \(U_t:\{u_1,u_2,...u_{t}\}\)，\(u_t\)表示从状态\(x_{t-1}\)过渡到\(x_t\)的控制
- 观测序列：\(Z_t:\{z_0,z_1,....z_{t}\}\)，\(z_t\)表示在状态\(x_t\)下的观测
我们不希求通过\(z_t\)来求使得\(P(x_t|z_t)\)最大的\(x_t\)，而是需要求： \[ \begin{equation}\label{post} x_t^* = \mathop{\arg \max}_{x_t}P(x_t|Z_t,U_t) \end{equation} \] 实际上，可以说公式\(\eqref{post}\)定义的是一个后验概率。也就是最大化一个后验概率了，啊这不就是 贝叶斯滤波吗？确实，粒子滤波就是贝叶斯滤波的一个非参数实现。
但是一般来说，没有马尔可夫假设，事情会变得很复杂，如果我们需要对一大串控制 / 状态序列进行处理，应该怎么做？
2.2 粒子与采样
这是不是自然计算？粒子是一个个的可能状态：\(x_{t,1},x_{t,2},...x_{t,k}\)，假设我们有k个假设的状态。此时我们应该运用贝叶斯学派知识，首先由信息包含关系（之前的观测与控制包含在状态\(x_{t-1,i}\)中） \[ \begin{equation}\label{info} P(x_{t,i}|Z_t,U_t,x_{0,i})=P(x_{t,i}|z_t,u_t,x_{t-1,i}) \end{equation} \] 接下来的贝叶斯滤波变换可以说很妙了： \[ \begin{equation} P(x_{t,i}|z_t,u_t,x_{t-1,i}) = \frac{P(x_{t,i}, z_t,u_t,x_{t-1,i})}{P(z_t,u_t,x_{t-1,i})} \end{equation} \] 既然是要使用贝叶斯，那么就要想办法把观测和状态互换位置： \[ \begin{equation}\label{eqn4} \frac{P(x_{t,i}, z_t,u_t,x_{t-1,i})}{P(z_t,u_t,x_{t-1,i})}=\frac{P(z_t|x_{t,i},u_t,x_{t-1,i})P(x_{t,i},u_t,x_{t-1,i})}{P(z_t|u_t,x_{t-1,i})P(x_{t-1,i},u_t)} =\frac{P(z_t|x_{t,i},u_t,x_{t-1,i})P(x_{t,i}|u_t,x_{t-1,i})}{P(z_t|u_t,x_{t-1,i})} \end{equation} \] 注意，我们有如下两个论断成立： \[ \begin{align} & P(z_t|x_{t,i},u_t,x_{t-1,i}) = P(z_t|x_{t,i})\label{within} \\ & P(x_{t,i}|u_t,x_{t-1,i}) \text{ is a known prior}\label{prior} \end{align} \] 公式\(\eqref{within}\)说的是：观测\(z_t\)只与当前状态\(x_t\)有关。论断\(\eqref{prior}\)指的是，由控制引起的状态转移误差分布是已知的（比如本实现中，我假设我的控制噪声是白噪声，那么此处就是一个高斯项）。
那么公式\(\eqref{eqn4}\)可以被进一步化简为下式，其中\(\eta\)是我们不关心的归一化因子。 \[ \begin{equation}\label{eqn7} \eta P(z_t|x_{t,i})P(x_{t,i}|u_t,x_{t-1,i}) \end{equation} \] 那么可以这么理解：状态粒子i在第t时刻的观测\(z_t\)下，拥有状态\(x_{t,i}\)的概率为\(\eqref{eqn7}\)，包含两个组份：
- 状态粒子与观测的契合程度（给定状态能生成对应观测吗）\(P(z_t|x_{t,i})\)
- 控制状态转移噪声先验\(P(x_{t,i}|u_t,x_{t-1,i})\)
在2.5小节里，我简单地说一下我是如何对这个问题进行建模的。
2.3 分布变换
粒子滤波实际上是用粒子与采样来近似任意分布，在公式\(\eqref{eqn7}\)中，已经说明了\(P(x_{t,i}|x_{t-1,i},z_t,u_t)\)的组成成分。粒子滤波实际上还做了一个这样的事情，我们知道概率论中，概率是需要归一化的，而归一化因子在连续PDF中一般都涉及到积分。很多时候，积分是很难的，不一定能积出来。要获取目标分布，就存在一定难度了。这个时候，粒子滤波可以通过一个简单的建议分布，去推导一个复杂的目标分布。
我们重新定义一下问题。\(P(x_{t,i}|x_{t-1,i},z_t,u_t)\)是当前点的存在置信度，而我们希望，算法可以是迭代式（马尔可夫）的，也就是要从上一个迭代的粒子对应的置信度，计算本次迭代对应粒子的置信度。于是根据控制状态转移分布，可以得到建议分布： \[ P(x_{t,i}|x_{t-1,i},z_t,u_t)=P(x_{t,i}|x_{t-1,i},u_t)P(x_{t-1,i}|x_{t-2,i},z_{t-1},u_{t-1}) \] 可以发现，等号右边第二项就是上一次迭代对应粒子的分布。根据状态转移进行了分布变换，得到了新的建议分布。但是我们并非直接接受新的建议分布，为什么？
状态的转移带来新的信息。通俗地说，到一个新的地点，就可以从不同的角度观察环境。
我们可以通过新的观测，来衡量，这些新的点是否适合？原来的粒子，是分布的采样。那么，符合新的观测的粒子，自然可以给一个大权重，而不符合新观测的粒子，自然也就不那么重视。这就涉及到对粒子进行加权以及重采样。
2.4 加权 重采样
我们希望给粒子impose一个权重。但是粒子已经是分布的一个例化了（采样），这个时候有两种方案：
- 每个粒子维护一个权重，每次更新权重
- 每次重新生成一群粒子，根据权重进行生成
第一种思想很好理解。比如在localization问题中，我在位姿空间中有很多位姿的估计粒子，那么最后粒子求加权平均应该就是结果。
而第二种思想，则是把权重隐含在粒子数量中，这种思想是真正符合“粒子是对应分布的采样”的。显然，在N维空间中某个区域粒子数多，自然此处分布就应该比较大。
直接进行随机的Metropolis-Hasting采样，需要的时间开销比较大？为什么？根据这篇文章[2]，里面说：
简单随机重采样：（1）生成N个均匀分布随机数（2）随机数数列排序（3）根据随机数数列以及粒子的权重进行重采样。
这个步骤是：设点i的权重是\(w_i\)，生成的随机数数列是\(y_i\)。则对于所有的\(y_i\)，如果\(y_i\)落在\(\left(\sum_{k=1}^iw_k,\sum_{k=1}^{i+1}w_k\right]\)中，那么对应粒子的采样数+1。可知这样的采样方法，复杂度是\(O(NlogN)\)，毕竟需要排序，但是这是完全随机的。完全随机的两个坏处就体现出来了：
- 不能保证数据集的遍历性，有些数据可能根本没有办法被采样到
- 计算复杂度大，大型采样时间消耗大。
更好的算法，这里就不再赘述了，详见《概率机器人》第四章。我的实现中，使用的就是《概率机器人》中的“低方差采样”，时间复杂度为O(N)，并且是遍历性的。
重采样的目的就是使得建议分布转化为目标分布。对于粒子滤波，如果我们这样理解问题：滤波的完整迭代过程（从0到收敛有t个迭代），那么希望最大化： \[ \begin{equation}\label{new_obj} P(x_{0:t,i}|z_{1:t},u_{1:t}) \end{equation} \] 以localization任务来说。如果一个状态是比较正确的，那么不管如何迭代，其置信度在不同的迭代中都应该比较大。那么根据公式\(\eqref{eqn7}\)，可以展开为类似的结构（这是目标分布）： \[ \begin{equation}\label{eqn11} \eta P(z_t|x_{0:t,i}, z_{1:t},u_{1:t})P(x_{0:t,i}|u_t,z_{1:t-1,i}) \end{equation} \] 进一步根据贝叶斯和马尔可夫性，可以作进一步展开，可得到建议分布与目标分布之比与\(P(z_t|x_{t_i})\)成正比。
2.5 简单建模
此部分，我简要介绍一下在基于Volume2D Shader的激光雷达模拟器中，如何建模一些分布。最为重要的首先就是似然\(P(z_t|x_t)\)，也即，给定状态下，产生观测\(z_t\)的可能性。此处我将其建模为：
- 给定\(x_t\)（状态，也即粒子对应的位姿），我在此处模拟一个激光点云（因为已知地图），与实际的观测\(z_t\)进行比较。相似则对应的概率大，不相似则概率小。
- 那么相似性的比较，则是基于模拟激光点云对应角度上的range差值。其实只要理解似然在此处的作用，这个模型很好建立。
另一方面，是状态转移模型。此处直接使用了一个方差与移动大小相关的高斯噪声。
III. 结果
 |
 |
|---|---|
| 长走廊问题 初始化 | 长走廊问题 第三次迭代（移动3次） |
 |
 |
| 长走廊问题 第九次迭代（移动9次） | 长走廊问题 第十六次迭代（移动16次） |
对于不完全的长走廊问题（虽然相似但是还有一定区别的环境），粒子滤波还是可以应对的。（2000个粒子）。
Reference
[1] Probability Robotics (Sebastian Thrun, et al)
[2] 粒子滤波常用重采样算法分析比较，范澎湃等