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

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：
# 输入：
#       概率
# 输出：
#       格式化输出
def show(p):
    rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
    print('[' + ',\n '.join(rows) + ']')

# sense:
#   输入：
#       probs 当前概率分布
#       term  观察到的特征
#   输出：
#       p     添加滤波后的概率分布
def sense(probs, term, sensor_right) :
    s = 0
    for i in range(len(probs)) :
        for j in range(len(probs[0])) :
            # 判断是不是目标特征
            # 方便为后面添加不同的滤波器
            hit = (colors[i][j] == term)
            probs[i][j] *= (hit * sensor_right + (1 - hit) * (1 - sensor_right))
            s += probs[i][j]

    for i in range(len(probs)) :
        for j in range(len(probs[0])) :
            probs[i][j] /= s 
    return probs

# move:
#   输入：
#       probs  当前概率分布
#       motion 移动指令
#   输出：
#       p      卷积后的概率分布
def move(probs, motion, p_move) :
    q = []
    x = len(probs)
    y = len(probs[0])
    #竖着移动
    if motion[0] != 0 :
        for i in range(x) :
            tmp = []
            for j in range(y) :
                s = p_move * probs[(i - motion[0]) % x][j]
                s += (1 - p_move) * probs[(i - motion[0] + 1) % x][j]
                tmp.append(s)
            q.append(tmp)
    #横着移动
    else :
        for i in range(x) :
            tmp = []
            for j in range(y) :
                s = p_move * probs[i][(j - motion[1]) % y]
                s += (1 - p_move) * probs[i][(j - motion[1] + 1) % y]
                tmp.append(s)
            q.append(tmp)
    return q 

#localize:
#   输入：
#       colors          二维空间
#       measurements    观察到的特征
#       motions         移动
#       sensor_right    滤波器概率
#       p_move          移动卷积概率
#   输出：
#       probs           概率分布
def localize(colors, measurements, motions, sensor_right, p_move) :
    pinit = 1.0 / float(len(colors)) / float(len(colors[0]))
    p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]
    for i in range(len(motions)) :
        p = move(p, motions[i], p_move)
        p = sense(p, measurements[i], sensor_right)
    return p

# colors:
#   二维空间特征
colors = [['R','G','G','R','R'],
          ['R','R','G','R','R'],
          ['R','R','G','G','R'],
          ['R','R','R','R','R']]

# measurements:
#   观察到的特征            
measurements = ['G','G','G','G','G']
# motions:
#   移动轨迹
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]

#############################################################
# 对于一下的例子，应该输出：
# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],
#  [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],
#  [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],
#  [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]
# ( +/- 0.001 的误差)
p = localize(colors, measurements, motions, sensor_right = 0.7, p_move = 0.8)
show(p)