47 lines
No EOL
1.6 KiB
Python
47 lines
No EOL
1.6 KiB
Python
def localize(colors,measurements,motions,sensor_right,p_move):
|
|
# initializes p to a uniform distribution over a grid of the same dimensions as colors
|
|
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(measurements)):
|
|
p = move(p, motions[i], p_move, colors)
|
|
p = sense(p, measurements[i], sensor_right, colors)
|
|
return p
|
|
|
|
def sense(p, measurements, pHit, colors):
|
|
pMiss = 1. - pHit
|
|
q = [[0 for row in range(len(colors[0]))] for col in range(len(colors))]
|
|
for x in range(len(p[0])):
|
|
for y in range(len(p)):
|
|
hit = (measurements == colors[y][x])
|
|
q[y][x] = (p[y][x] * (hit * pHit + (1-hit) * pMiss))
|
|
s = sum(map(sum,q))
|
|
for x in range(len(q[0])):
|
|
for y in range(len(q)):
|
|
q[y][x] = q[y][x] / s
|
|
return q
|
|
|
|
def move(p, U, p_move, colors):
|
|
p_stay = 1. - p_move
|
|
q = [[0 for row in range(len(colors[0]))] for col in range(len(colors))]
|
|
for x in range(len(q[0])):
|
|
for y in range(len(q)):
|
|
s = p_move * p[(y-U[0]) % len(p)][(x-U[1]) % len(p[0])]
|
|
s += p_stay * p[y][x]
|
|
q[y][x] = s
|
|
return q
|
|
|
|
def show(p):
|
|
#rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
|
|
#print '[' + ',\n '.join(rows) + ']'
|
|
|
|
colors = [['R','G','G','R','R'],
|
|
['R','R','G','R','R'],
|
|
['R','R','G','G','R'],
|
|
['R','R','R','R','R']]
|
|
measurements = ['G','G','G','G','G']
|
|
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
|
|
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)
|
|
|
|
show(p) |