Monte Carlo Localization [Python] / Interfaces / Torr Vickers

This program was created for the CS373 (Building a Robotic Car) class at Udacity.com. Using a particle filter, this program enables an AI unit to localize itself within a grid world, given a map of its environment (the grid) and two lists.

One list represents the movements the AI unit makes, and the second list represents its measurement after one movement. The program takes into account sensor inaccuracy and the possibility of unsuccessful movement.

Both the sensor inaccuracy and the movement accuracy can be manipulated before execution, in order to view different results.

The program outputs a matrix that is the same size as its grid world environment. Each cell in this matrix contains the probability that the AI unit believes itself to be in that cell.

=====================================================================

#-------------------------------------------------------------------------------
# Name: Monte Carlo Localization
# Purpose: Enables artificial intelligence to localize itself within a
# grid world using a particle filter.
#
# Author: Torr
#
# Created: 03/27/2012
#-------------------------------------------------------------------------------

#Function Definitions
#**************************************************

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

def set_uniformDistribution(world):
matrix = [[(1./(rows * columns)) for col in range(columns)] for row in range(rows)]
return matrix

def sense(world, measurement):
#sense the environment
q = [[0 for col in range(columns)] for row in range(rows)]
for i in range(rows):
for j in range(columns):
if measurement == colors[i][j]:
q[i][j] = world[i][j] * pHit
else:
q[i][j] = world[i][j] * pMiss

#and normalize
s = 0
for k in range(rows):
s = s + sum(q[k])
for l in range(rows):
for m in range(columns):
#division by zero check
if s > 0:
q[l][m] = q[l][m] / s
else:
s = 0.000000000000001
q[l][m] = q[l][m] / s
return q

def move(world, motion):
#extract the movement directions
motion = str(motion)
leftPos = motion.find('[')
rightPos = motion.find(']')
commaPos = motion.find(',')
x = int(motion[leftPos + 1: commaPos])
y = int(motion[commaPos + 1: rightPos])

#make the move
q = [[0 for col in range(columns)] for row in range(rows)]
for i in range(rows):
for j in range(columns):
probability = pExact * world[(i - x) % rows][(j - y) % columns]
probability = probability + pOvershoot * world[(i - x - 1) % rows][(j - y - 1) % columns]
probability = probability + pUndershoot * world[(i - x + 1) % rows][(j - y + 1) % columns]
q[i][j] = probability
return q

#Main Program / Testing
#**************************************************

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

#sensor measurements
measurements = ['green', 'green', 'green' ,'green', 'green']

#AI motions [x, y]
#for x, -1 is left and +1 is right (row movement)
#for y, -1 is up and +1 is down (column movement)
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]

#accuracy settings
sensor_right = 0.8
p_move = 0.7

#create the grid space
p = [[]]
rows = 4
columns = 5
p = set_uniformDistribution(p)

#set accuracy values
pHit = sensor_right
pMiss = (1 - sensor_right)
pExact = p_move
pOvershoot = (1 - p_move) / 2
pUndershoot = (1 - p_move) / 2

#apply measurements and motions
#assumes that measurements and motions are of the same length
for i in range(len(measurements)):
p = sense(p, measurements[i])
p = move(p, motions[i])

#display the results
#this will display a grid of the same size as the environment
# with each cell holding the probability that the AI believes
# itself to be in that cell
show(p)

#for debugging
#print("pHit = " + str(pHit))
#print("pMiss = " + str(pMiss))
#print("pExact = " + str(pExact))
#print("pOvershoot = " + str(pOvershoot))
#print("pUndershoot = " + str(pUndershoot))

Interfaces: Monte Carlo Localization [Python]

Enables an AI unit to localize itself within a simple grid world, using a particle filter.