Source

udacity373_code / unit1 / u1-hw1_localization.py

Full commit
#u1-hw1_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
'''
colors = [['green', 'green', 'green'],
          ['green', 'red',   'red'],
          ['green', 'green', 'green']]
          
measurements = ['red', 'red']
motions = [[0,0], [0,1]]

sensor_right = 0.8
p_move = 1
'''
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
def init_p(colors):
    p = []
    n_elem = len(colors) * len(colors[0])
    for row in colors:
        p.append([(1.0/n_elem) for i in row])

    return p

def copy_matrix(src):
    q = init_p(colors)
    for row in range(0, world_size[0]):
        for col in range(0, world_size[1]):
            q[row][col] = src[row][col]
    return q

def normalize(q):
    #normalize probability matrix
    total = sum([sum(row) for row in q])
    if total <= 0:
        print "Cant normalize with factor: 0"
        raise ValueError, "Cant normalize with factor: 0"
        return -1
        
    #normalize if possible
    for row in range(0,world_size[0]):
        for col in range(0, world_size[1]):
                q[row][col] /= total
                    
    return q


def sense(p2, Z):
    #found prior vals
    for row in range(len(colors)):
        for col in range(len(colors[row])):
            hit = (Z == colors[row][col])
            #p[i] * (hit * pHit + (1-hit) * pMiss)
            p2[row][col] *= sensor_right * hit + (1- sensor_right) * (1 - hit)
                
    return normalize(p2)

def move(p, Pos):
    q = copy_matrix(p) #to prevent side-effects
    #q=p
    total = 0.0 
    for row in range(0, world_size[0]):
        print "------------"
        for col in range(0, world_size[1]):
            #q[row][col] *= (1-p_move) * p[row][col] + (p_move) * p[prev_y][prev_x]
            #q[prev_y][prev_x] *= p_move
            prev_y = lambda pos, step=1: (((row-step*pos) % world_size[0]) + world_size[0]) % world_size[0]
            prev_x = lambda pos, step=1: (((col-step*pos) % world_size[1]) + world_size[1]) % world_size[1]
            print("({0}:{1}={2}:{3})".format(row,col, prev_y(Pos[0]), prev_x(Pos[1])))
            
            q[row][col] = p[prev_y(Pos[0])][prev_x(Pos[1])] * p_move
            q[row][col] += p[row][col] *  (1- p_move)
    
    return q

p = []
p = init_p(colors)
world_size = [len(p), len(p[0])] #[row_id, col_id]
print "World size:", world_size
#print "initial:", p
for k in range(len(measurements)):
    #print "run:", k+1
    p = move(p, motions[k])
    #print "moved: \n", p
    p = sense(p, measurements[k])
    #print "sensed: \n", p
    
#print ""    

#Your probability array must be printed 
#with the following code.
show(p)