The code skeleton for markov localization of robotic car

MonteCarloLocalization.py

# the 'map'
colors = [['red', 'green', 'green', 'red' , 'red'],
          ['red', 'red', 'green', 'red', 'red'],
          ['red', 'red', 'green', 'green', 'red'],
          ['red', 'red', 'red', 'red', 'red']]

# the results of sensor
measurements = ['green', 'green', 'green' ,'green', 'green']

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

# probability of sensors doing right
sensor_right = 0.7

# probability of moving success
p_move = 0.8

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

# init p with uniform distribution
p = []
uni = 1.0 / (len(colors) * len(colors[0]))
for row in colors:
    newRow = []
    p.append(newRow)
    for col in row:
        newRow.append(uni)

def sense(p, mm):
    q = []
    for (rowIndex, row) in enumerate(p):
        q.append([])
        for (colIndex, col) in enumerate(row):
            if mm == colors[rowIndex][colIndex]:
                q[rowIndex].append(col * sensor_right)
            else:
                q[rowIndex].append(col * (1 - sensor_right))
    # normalization
    normalizer = sum(map(sum, q))
    return map(lambda r: map(lambda x: x / normalizer, r), q)
    
def move(p, motion):
    q = []
    for (rowIndex, row) in enumerate(p):
        q.append([])
        for (colIndex, col) in enumerate(row):
            q[rowIndex].append(col * (1-p_move) + p_move * p[(rowIndex - motion[0]) % len(p)][(colIndex - motion[1]) % len(row)])
    return q

for (motion, mm) in zip(motions, measurements):
    p = move(p, motion)
    p = sense(p, mm)

show(p)