机器人概率定位学习笔记第四篇_使用python进行机器人概率定位

本笔记学习于优达学城 Artificial Intelligence for Robotics课程

1，机器人定位问题其实就是机器人运动与感知问题，在一个世界中感知，运动，不断循环。如下图

2，下面我们正式开始写一个完整的机器人概率定位系统，首先假设这个世界

[green red red green green]

机器人开始感知第一次感知到红色，然后向右走了一步，然后感知到绿色，然后向又右走了一步，问最后机器人在那个方格的概率最大，很明显机器人在第四个绿色的方格中概率最大，下面我们用代码来实现吧

2.1 获取机器人的感知到某种颜色　然后在各个位置处的概率

#Given the list motions=[1,1] which means the robot 
#moves right and then right again, compute the posterior 
#distribution if the robot first senses red, then moves 
#right one, then senses green, then moves right again, 
#starting with a uniform prior distribution.

p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1

def sense(p, Z):
    q=[]
    for i in range(len(p)):
        hit = (Z == world[i])
        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    s = sum(q)
    for i in range(len(q)):
        q[i] = q[i] / s
    return q

def move(p, U):
    q = []
    for i in range(len(p)):
        s = pExact * p[(i-U) % len(p)]
        s = s + pOvershoot * p[(i-U-1) % len(p)]
        s = s + pUndershoot * p[(i-U+1) % len(p)]
        q.append(s)
    return q
for i in range(len(motions)):
    p=sense(p,measurements[i])
    p=move(p,motions[i])
print p         

[0.21157894736842103, 0.1515789473684211, 0.08105263157894739, 0.16842105263157897, 0.3873684210526316]