滤波笔记三：粒子滤波

Reference:

1. 如何直观理解粒子滤波并进行Python编程实践
2. 如何通俗地理解「蒙特卡洛方法」，它解决问题的基本思路是什么，目前主要应用于哪些领域？
3. 通俗地解释粒子滤波器
4. Particle Filters for Positioning, Navigation and Tracking

上文中已经学习了卡尔曼滤波，卡尔曼滤波在通讯、导航、控制等领域得到了广泛的应用，也取得了感人的效果。但也不得不说，卡尔曼滤波的线性、高斯假设性太强，实际系统中并不能直接使用。即使是利用线性化手段，也要求原系统线性程度较好，否则效果也会较差。同时，我们知道高斯函数为单峰函数，这也为其带来了局限性。例如在定位问题中，卡尔曼滤波便仅适用于位置追踪（position tracking），从其高斯分布的递推估计过程也可以看出。而针对全局定位（global localization）和机器人绑架问题（kidnapped robot problem），卡尔曼便不易解决。

因此，现引出一种十分流行的非参数滤波——粒子滤波（particle filter）。

1. 概述

已知有一个机器人，已知商场地图并能观测到周围长什么样，现在想知道机器人位于地图的哪个位置。
前面提到机器人是知道地图的，机器人先猜自己位置，然后根据自己当前的观测来缩小猜测范围。假设下图就是一个商场地图(左)，红点是机器人猜测自己可能在的位置，蓝色的是建筑物。这时机器人根据自己的摄像头或者雷达观测周围环境，比如它能够看到圆形建筑物，（由于机器人已知商场的地图）那么它就可以确定自己在商场的中央区域。这样就可以缩小猜的范围。

它再走两下发现，自己右侧有三个建筑物，猜测的范围又缩小了(中)。

就这样根据观测和地图来不断缩小猜测范围，最终可以确定机器人所在的位置(右)。

2. 稍微难一点的理解

假设我们有一个小车，可以在2D平面上移动。我们离散化整个过程，如在出发后的 $1s,2s,3s$ 使用粒子滤波定位小车的位置。

粒子滤波定位小车的过程分为三步，需要三个信息：

$\circ$ 三个信息

1. 车上一秒在哪
2. 车的运动是什么
3. 测量结果，如 GPS

$\circ$ 三个步骤

1. Prediction Step
2. Innovation Step
3. Re-sampling Step

2.1 Prediction Step（预测）

用到上一步的结果和运动。这个过程可以用马尔可夫链简化，也就是说，这一步的状态由上一步的状态+转移过程就可以确定。

我们用“例子”点来表示车的位置，也就是上图的 ×。上一步的位置，我们用 × 来表示，它们都是车的可能位置，如上图的(1,5),(3,5),(3,7),(5,5)。

这些位置听谁的来给车辆定位呢？我们可以给每个位置定一个权重，然后加权平均一下。在预测这步权重都是一样的，现在有四个粒子，每个权重就是 $0.25$。

也就是我们有四个粒子 $x_1$，$x_2$，$x_3$，$x_4$，$x$[(横坐标x，纵坐标y)，权重w]:
$x_1[(1,5),0.25]$
$x_2[(3,5),0.25]$
$x_3[(3,7),0.25]$
$x_4[(5,5),0.25]$

我们有个运动，这个运动可能是轮速传感器或IMU等得来的。

假设我们现在知道了运动是$(5,4)$，但是还有个误差。预测这步就是所有权重不变，在状态（运动）加上这个运动的结果，如果知道噪声大小，可以在结果上加一个随机数：
$x_1[(6,9),0.25]$ ----> $x_1[(6.1,9.2),0.25]$
$x_2[(8,9),0.25]$ ----> $x_2[(7.9,9),0.25]$
$x_3[(8,11),0.25]$ ----> $x_3[(8.3,10.8),0.25]$
$x_4[(10,9),0.25]$ ----> $x_4[(10.1,8.9),0.25]$

2.2 Innovation Step

这时我们有个 GPS，测量值是下图绿色的点，假设测量值遵循高斯分布：

现在要考虑的是，如何将测量值和预测的粒子信息融合在一起？
按照实际测量值和预测测量值之间的不匹配来重新分配权重，该权重被称为重要性权重(importance weight)。高斯分布离中心点越近权重越大，则上图有 $x_2<x_3<x_1<x_4$，这时权重大于 $1$ 了，需要归一化一下，得：
$x_1[(6.1,9.2),0.4]$ ----> $x_1[(6.1,9.2),0.26]$
$x_2[(7.9,9),0.5]$ ----> $x_2[(7.9,9),0.33]$
$x_3[(8.3,10.8),0.35]$ ----> $x_3[(8.3,10.8),0.23]$
$x_4[(10.1,8.9),0.25]$ ----> $x_4[(10.1,8.9),0.16]$

从下图中可以看出，我们有很多不同粒子，以及一个特定的测量值。每个粒子权重都不同，有的看起来可能性很高，其他可能看起来可能性很低

2.3 Resampling step

重采样是指，从旧粒子中随机抽取 $N$ 个新粒子，同时根据重要性权重进行替换。

上个步骤中得到的权重，在这步用上了。现在，我们随机让这些粒子存活，它们存活的概率和它们的权重成正比。如果某个粒子权重很大，那么它存活的概率会超出权重很小的粒子 如，
$x_1[(6.1,9.2),0.26]$
$x_2[(7.9,9),0.33]$
$x_3[(8.3,10.8),0.23]$
$x_4[(10.1,8.9),0.16]$
比如 $x_2$ 的概率是 $0.33$，那么新摇出来的粒子可能有两个 $x_2$，因为它概率最大，$x_4$的概率比较小可能一个都没有。

我们可以抽取同一个粒子的多次，假设之前的粒子有 $N$ 个，此时的选取也要有 $N$ 次。摇出来的新粒子权重相等：
$x_1[(6.1,9.2),0.25]$
$x_2[(7.9,9),0.25]$
$x_3[(8.3,10.8),0.25]$
$x_4[(7.9,9),0.25]$
最终的估计值可以是四个粒子的平均。重采样环节后，这里的粒子很可能会继续存活，几率会高很多很多倍----这样粒子会聚集在后验概率比较高的区域附近。

粒子滤波完。

3. 概念

3.1 蒙特卡洛

蒙特卡洛(Monte Carlo)是一大类随机算法的总称，它们通过随机样本来估算真实值。可以通过一个例子来理解：

• 近似 $\pi$ 值

我们知道 $\pi$ 约等于 $3.14159265$。现在假装不知道，想要求出 $\pi$ 的近似估算值。这里我们使用蒙特卡洛近似 $\pi$ 值。

假设我们有一个随机数生成器，可以均匀生成 $-1$ 到 $+1$ 之间的数。每次生成两个随机数当作平面坐标系上的点 $(x,y)$，我们重复抽样 $n$ 次，得到 $n$ 个正方形内的点。

由于抽样是均匀的，一个点落在圆里面的概率显然是圆面积与正方形面积的比，即 $p=\frac{a_2}{a_1}=\frac{\pi }{4}$。

假设随机抽样了 $n$ 个点，设园内点的数量为随机变量 $M$，则 $M$ 的期望等于 $\mathbb{E}[M]=pn=\frac{\pi n}{4}$。

注意这里说的是期望而不是实际发生的结果。如果抽了 $n=5$ 个点，那么期望有 $\mathbb{E}[M]=\frac{5\pi }{4}$ 个点落在圆内，但实际观测值 $m$ 可能等于 $0,1,2,3,4,5$ 中的任何一个。

我们均匀随机抽样得到 $n$ 个点，通过圆的方程对每个点做判别，发现有 $m$ 个点落在圆里面。假如 $n$ 非常大，那么随机变量 $M$ 的真实观测值 $m$ 就会非常接近期望 $m\approx \frac{\pi n}{4}$，即 $\pi \approx \frac{4m}{n}$。

大数定律保证了蒙特卡洛的正确性：当 $n$ 趋于无穷，$\frac{4m}{n}$ 趋于 $\pi$。其实还能进一步用概率不等式分析误差的上界。使用 Bernstein 不等式，可以证明出结论：
$$|\frac{4m}{n}-\pi |=O(\frac{1}{\sqrt{n}})$$

这个不等式说明 $\frac{4m}{n}$ 会收敛到 $\pi$，收敛率是 $\frac{1}{\sqrt{n}}$。然而这个收敛率并不快：样本数量 $n$ 增加一万倍，精度才能提高一百倍。