Torr Vickers

Projects: Monte Carlo Localization [Python]

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

    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))
    Available for freelance