机器人定位 - 蒙特卡罗定位（直方滤波算法）

---

wiki上的介绍 https://en.wikipedia.org/wiki/Monte_Carlo_localization

Udacity上的课程 https://cn.udacity.com/course/artificial-intelligence-for-robotics–cs373/
下面的例子来自优达学城

---

原理

假设一个机器人在一个一维的世界里，具体假设场景[R,R,G,R,R]。其中R代表红色，G代表绿色。
机器人有一个全局的分布表。

1.首先机器人为所有特征点给一个平均概率，代表最混乱的情况。

• 1/5 = 0.2， 所以机器人现在全局表是[0.2, 0.2, 0.2, 0.2, 0,2]，代表机器人有0.2的概率处在对应的位置。

2.当机器人感知到对应特征的时候，会添加一个滤波器

• 机器人每感知到一个特征就会为所有的位置添加一个滤波器
• 例如 p=0.8， (1-p)=0.2，滤波器自己定义
  原始状态是[0.2, 0.2, 0.2, 0.2, 0.2]，当感知到R特征的时候，就为所有R的位置添加0.8的滤波器. 0.2*0.8 = 0.16, 0.2 * 0.2 = 0.04
• 然后进行归一化的处理。这里涉及到贝叶斯公式的原理。
• 最后的结果大家自己算吧。

3.然后机器人向前移动

因为机器人很难确定自己具体行进过程，所以当机器人前进的时候，需要进行卷积运算。
- 例如 p0=0.1 p1=0.8 p2=0.1,代表机器人有0.1的概念走到目标点的后面，有0.8的概率刚好到目标点，0.1的概率会超过目标点。
- 假设现在是[0.6, 0, 0, 0],机器人向前走两步，则 0.1 * 0.6 = 0.06， 0.8 * 0.6 = 0.48,结果是[0, 0.06, 0.48, 0.06]，最后归一化处理。
- 需要注意的是，移动是对于全局表中所有概率都进行运算，一移动整个表都会移动的。移动完再进行归一化。这里涉及到全概率公式。

4.最后结果取最大值就是机器人的位置。

代码

这里选的是Udacity的习题，我的结果对了，但是就是提交失败，这是仅提供给大家参考

# show：
# 输入：
#       概率
# 输出：
#       格式化输出