HW1.4 Localization Problem

localization.py

colors = [['red', 'green', 'green', 'red' , 'red'],
          ['red', 'red', 'green', 'red', 'red'],
          ['red', 'red', 'green', 'green', 'red'],
          ['red', 'red', 'red', 'red', 'red']]

measurements = ['green', 'green', 'green' ,'green', 'green']


motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]

sensor_right = 0.7

p_move = 0.8

def show(p):
    for i in range(len(p)):
        print p[i]

#DO NOT USE IMPORT
#ENTER CODE BELOW HERE
#ANY CODE ABOVE WILL CAUSE
#HOMEWORK TO BE GRADED
#INCORRECT

w = len(colors[0])
h = len(colors)

p = [[1./(w * h) for k in range(w)] for l in range(h)]

def norm(p):
    s = 0
    for row in p:
        s += sum(row)
    return [[v/s for v in r] for r in p]

def sense(p, Z):
    for r in range(h):
        for c in range(w):
            if colors[r][c] == Z:
                p[r][c] *= sensor_right
            else:
                p[r][c] *= (1-sensor_right)
    return norm(p)

def move(p, m):
    y,x = m
    if (y,x)==(0,0):
        return p
    z = [[0 for k in range(w)] for j in range(h)]
    for r in range(h):
        for c in range(w):
            z[r][c] += (1-p_move)*p[r][c]
            if y==0:
                z[r][(c+x) % w] += p_move*p[r][c % w]
            else:
                z[(r+y) % h][c] += p_move*p[r % h][c]
    return z
    
for i in range(len(motions)):
    p = move(p, motions[i])
    p = sense(p, measurements[i])





#Your probability array must be printed 
#with the following code.

show(p)