标签：8.3 粒子 0.25 滤波 笔记 6.1 10.8 7.9 9.2
滤波笔记三：粒子滤波
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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。