滤波笔记三：粒子滤波
作者：互联网
滤波笔记三：粒子滤波
Reference:
上文中已经学习了卡尔曼滤波，卡尔曼滤波在通讯、导航、控制等领域得到了广泛的应用，也取得了感人的效果。但也不得不说，卡尔曼滤波的线性、高斯假设性太强，实际系统中并不能直接使用。即使是利用线性化手段，也要求原系统线性程度较好，否则效果也会较差。同时，我们知道高斯函数为单峰函数，这也为其带来了局限性。例如在定位问题中，卡尔曼滤波便仅适用于位置追踪（position tracking），从其高斯分布的递推估计过程也可以看出。而针对全局定位（global localization）和机器人绑架问题（kidnapped robot problem），卡尔曼便不易解决。
因此，现引出一种十分流行的非参数滤波——粒子滤波（particle filter）。
1. 概述
已知有一个机器人，已知商场地图并能观测到周围长什么样，现在想知道机器人位于地图的哪个位置。
前面提到机器人是知道地图的，机器人先猜自己位置，然后根据自己当前的观测来缩小猜测范围。假设下图就是一个商场地图(左)，红点是机器人猜测自己可能在的位置，蓝色的是建筑物。这时机器人根据自己的摄像头或者雷达观测周围环境，比如它能够看到圆形建筑物，（由于机器人已知商场的地图）那么它就可以确定自己在商场的中央区域。这样就可以缩小猜的范围。
它再走两下发现，自己右侧有三个建筑物，猜测的范围又缩小了(中)。
就这样根据观测和地图来不断缩小猜测范围，最终可以确定机器人所在的位置(右)。
2. 稍微难一点的理解
假设我们有一个小车，可以在2D平面上移动。我们离散化整个过程，如在出发后的 1 s , 2 s , 3 s 1s,2s,3s 1s,2s,3s 使用粒子滤波定位小车的位置。
粒子滤波定位小车的过程分为三步，需要三个信息：
∘ \circ ∘ 三个信息
- 车上一秒在哪
- 车的运动是什么
- 测量结果，如 GPS
∘ \circ ∘ 三个步骤
- Prediction Step
- Innovation Step
- Re-sampling Step
2.1 Prediction Step（预测）
用到上一步的结果和运动。这个过程可以用马尔可夫链简化，也就是说，这一步的状态由上一步的状态+转移过程就可以确定。
我们用“例子”点来表示车的位置，也就是上图的 ×。上一步的位置，我们用 × 来表示，它们都是车的可能位置，如上图的(1,5),(3,5),(3,7),(5,5)。
这些位置听谁的来给车辆定位呢？我们可以给每个位置定一个权重，然后加权平均一下。在预测这步权重都是一样的，现在有四个粒子，每个权重就是 0.25 0.25 0.25。
也就是我们有四个粒子
x
1
x_1
x1，
x
2
x_2
x2，
x
3
x_3
x3，
x
4
x_4
x4，
x
x
x[(横坐标x，纵坐标y)，权重w]:
x
1
[
(
1
,
5
)
,
0.25
]
x_1[(1,5),0.25]
x1[(1,5),0.25]
x
2
[
(
3
,
5
)
,
0.25
]
x_2[(3,5),0.25]
x2[(3,5),0.25]
x
3
[
(
3
,
7
)
,
0.25
]
x_3[(3,7),0.25]
x3[(3,7),0.25]
x
4
[
(
5
,
5
)
,
0.25
]
x_4[(5,5),0.25]
x4[(5,5),0.25]
我们有个运动，这个运动可能是轮速传感器或IMU等得来的。
假设我们现在知道了运动是
(
5
,
4
)
(5,4)
(5,4)，但是还有个误差。预测这步就是所有权重不变，在状态（运动）加上这个运动的结果，如果知道噪声大小，可以在结果上加一个随机数：
x
1
[
(
6
,
9
)
,
0.25
]
x_1[(6,9),0.25]
x1[(6,9),0.25] ---->
x
1
[
(
6.1
,
9.2
)
,
0.25
]
x_1[(6.1,9.2),0.25]
x1[(6.1,9.2),0.25]
x
2
[
(
8
,
9
)
,
0.25
]
x_2[(8,9),0.25]
x2[(8,9),0.25] ---->
x
2
[
(
7.9
,
9
)
,
0.25
]
x_2[(7.9,9),0.25]
x2[(7.9,9),0.25]
x
3
[
(
8
,
11
)
,
0.25
]
x_3[(8,11),0.25]
x3[(8,11),0.25] ---->
x
3
[
(
8.3
,
10.8
)
,
0.25
]
x_3[(8.3,10.8),0.25]
x3[(8.3,10.8),0.25]
x
4
[
(
10
,
9
)
,
0.25
]
x_4[(10,9),0.25]
x4[(10,9),0.25] ---->
x
4
[
(
10.1
,
8.9
)
,
0.25
]
x_4[(10.1,8.9),0.25]
x4[(10.1,8.9),0.25]
2.2 Innovation Step
这时我们有个 GPS，测量值是下图绿色的点，假设测量值遵循高斯分布：
现在要考虑的是，如何将测量值和预测的粒子信息融合在一起？
按照测量的概率分布来重新分配权重。高斯分布离中心点越近权重越大，则上图有
x
2
<
x
3
<
x
1
<
x
4
x_2<x_3<x_1<x_4
x2<x3<x1<x4：
x
1
[
(
6.1
,
9.2
)
,
0.4
]
x_1[(6.1,9.2),0.4]
x1[(6.1,9.2),0.4]
x
2
[
(
7.9
,
9
)
,
0.5
]
x_2[(7.9,9),0.5]
x2[(7.9,9),0.5]
x
3
[
(
8.3
,
10.8
)
,
0.35
]
x_3[(8.3,10.8),0.35]
x3[(8.3,10.8),0.35]
x
4
[
(
10.1
,
8.9
)
,
0.25
]
x_4[(10.1,8.9),0.25]
x4[(10.1,8.9),0.25]
这时权重大于
1
1
1 了，需要归一化一下，得：
x
1
[
(
6.1
,
9.2
)
,
0.26
]
x_1[(6.1,9.2),0.26]
x1[(6.1,9.2),0.26]
x
2
[
(
7.9
,
9
)
,
0.33
]
x_2[(7.9,9),0.33]
x2[(7.9,9),0.33]
x
3
[
(
8.3
,
10.8
)
,
0.23
]
x_3[(8.3,10.8),0.23]
x3[(8.3,10.8),0.23]
x
4
[
(
10.1
,
8.9
)
,
0.16
]
x_4[(10.1,8.9),0.16]
x4[(10.1,8.9),0.16]
2.3 Resampling step
上个步骤中得到的权重，在这步用上了。我们之前的那些粒子不用了，重新摇骰子摇几个粒子出来。一般来讲，粒子数量是不变的，都是能足够多就足够多。如，
x
1
[
(
6.1
,
9.2
)
,
0.26
]
x_1[(6.1,9.2),0.26]
x1[(6.1,9.2),0.26]
x
2
[
(
7.9
,
9
)
,
0.33
]
x_2[(7.9,9),0.33]
x2[(7.9,9),0.33]
x
3
[
(
8.3
,
10.8
)
,
0.23
]
x_3[(8.3,10.8),0.23]
x3[(8.3,10.8),0.23]
x
4
[
(
10.1
,
8.9
)
,
0.16
]
x_4[(10.1,8.9),0.16]
x4[(10.1,8.9),0.16]
比如
x
2
x_2
x2 的概率是
0.33
0.33
0.33，那么新摇出来的粒子可能有两个
x
2
x_2
x2，因为它概率最大，
x
4
x_4
x4的概率比较小可能一个都没有。
假设之前的每种可能性都摇到了，我们认为摇出来的新粒子权重相等：
x
1
[
(
6.1
,
9.2
)
,
0.25
]
x_1[(6.1,9.2),0.25]
x1[(6.1,9.2),0.25]
x
2
[
(
7.9
,
9
)
,
0.25
]
x_2[(7.9,9),0.25]
x2[(7.9,9),0.25]
x
3
[
(
8.3
,
10.8
)
,
0.25
]
x_3[(8.3,10.8),0.25]
x3[(8.3,10.8),0.25]
x
4
[
(
7.9
,
9
)
,
0.25
]
x_4[(7.9,9),0.25]
x4[(7.9,9),0.25]
最终的估计值可以是四个粒子的平均，粒子滤波完。
3. 概念
3.1 蒙特卡洛
蒙特卡洛(Monte Carlo)是一大类随机算法的总称，它们通过随机样本来估算真实值。可以通过一个例子来理解：
- 近似 π \pi π 值
我们知道 π \pi π 约等于 3.14159265 3.14159265 3.14159265。现在假装不知道，想要求出 π \pi π 的近似估算值。这里我们使用蒙特卡洛近似 π \pi π 值。
假设我们有一个随机数生成器，可以均匀生成 − 1 -1 −1 到 + 1 +1 +1 之间的数。每次生成两个随机数当作平面坐标系上的点 ( x , y ) (x,y) (x,y)，我们重复抽样 n n n 次，得到 n n n 个正方形内的点。
由于抽样是均匀的，一个点落在圆里面的概率显然是圆面积与正方形面积的比，即
p
=
a
2
a
1
=
π
4
p=\frac{a_2}{a_1}=\frac{\pi}{4}
p=a1a2=4π。
假设随机抽样了 n n n 个点，设园内点的数量为随机变量 M M M，则 M M M 的期望等于 E [ M ] = p n = π n 4 \mathbb{E}[M]=p n=\frac{\pi n}{4} E[M]=pn=4πn。
注意这里说的是期望而不是实际发生的结果。如果抽了 n = 5 n=5 n=5 个点，那么期望有 E [ M ] = 5 π 4 \mathbb{E}[M]=\frac{5\pi}{4} E[M]=45π 个点落在圆内，但实际观测值 m m m 可能等于 0 , 1 , 2 , 3 , 4 , 5 0,1,2,3,4,5 0,1,2,3,4,5 中的任何一个。
我们均匀随机抽样得到 n n n 个点，通过圆的方程对每个点做判别，发现有 m m m 个点落在圆里面。假如 n n n 非常大，那么随机变量 M M M 的真实观测值 m m m 就会非常接近期望 m ≈ π n 4 m \approx \frac{\pi n}{4} m≈4πn，即 π ≈ 4 m n \pi \approx \frac{4m}{n} π≈n4m。
大数定律保证了蒙特卡洛的正确性：当
n
n
n 趋于无穷，
4
m
n
\frac{4m}{n}
n4m 趋于
π
\pi
π。其实还能进一步用概率不等式分析误差的上界。使用 Bernstein 不等式，可以证明出结论：
∣
4
m
n
−
π
∣
=
O
(
1
n
)
|\frac{4m}{n}-\pi| = O(\frac{1}{\sqrt{n}})
∣n4m−π∣=O(n
1)
这个不等式说明 4 m n \frac{4m}{n} n4m 会收敛到 π \pi π，收敛率是 1 n \frac{1}{\sqrt{n}} n 1。然而这个收敛率并不快：样本数量 n n n 增加一万倍，精度才能提高一百倍。
三级目录
标签：8.3,粒子,0.25,滤波,笔记,6.1,10.8,7.9,9.2 来源： https://blog.csdn.net/qq_28087491/article/details/118086177