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

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

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



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

[green red red green green]

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

#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]