Commits

Anonymous committed 164c0a0 Draft

initial commit, added codes

Comments (0)

Files changed (66)

+=======================================
+Udacity 373 - programming autonmous car
+=======================================
+
+Python solutions for homeworks and exams.
+
+
+# --------------
+# USER INSTRUCTIONS
+#
+# Now you will put everything together.
+#
+# First make sure that your sense and move functions
+# work as expected for the test cases provided at the
+# bottom of the previous two programming assignments.
+# Once you are satisfied, copy your sense and move
+# definitions into the robot class on this page, BUT
+# now include noise.
+#
+# A good way to include noise in the sense step is to
+# add Gaussian noise, centered at zero with variance
+# of self.bearing_noise to each bearing. You can do this
+# with the command random.gauss(0, self.bearing_noise)
+#
+# In the move step, you should make sure that your
+# actual steering angle is chosen from a Gaussian
+# distribution of steering angles. This distribution
+# should be centered at the intended steering angle
+# with variance of self.steering_noise.
+#
+# Feel free to use the included set_noise function.
+#
+# Please do not modify anything except where indicated
+# below.
+
+from math import *
+import random
+
+# --------
+# 
+# some top level parameters
+#
+
+max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car.
+bearing_noise = 0.1 # Noise parameter: should be included in sense function.
+steering_noise = 0.1 # Noise parameter: should be included in move function.
+distance_noise = 5.0 # Noise parameter: should be included in move function.
+
+tolerance_xy = 15.0 # Tolerance for localization in the x and y directions.
+tolerance_orientation = 0.25 # Tolerance for orientation.
+
+
+# --------
+# 
+# the "world" has 4 landmarks.
+# the robot's initial coordinates are somewhere in the square
+# represented by the landmarks.
+#
+# NOTE: Landmark coordinates are given in (y, x) form and NOT
+# in the traditional (x, y) format!
+
+landmarks  = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format.
+world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
+
+# ------------------------------------------------
+# 
+# this is the robot class
+#
+
+class robot:
+
+    # --------
+    # init: 
+    #    creates robot and initializes location/orientation 
+    #
+
+    def __init__(self, length = 20.0):
+        self.x = random.random() * world_size # initial x position
+        self.y = random.random() * world_size # initial y position
+        self.orientation = random.random() * 2.0 * pi # initial orientation
+        self.length = length # length of robot
+        self.bearing_noise  = 0.0 # initialize bearing noise to zero
+        self.steering_noise = 0.0 # initialize steering noise to zero
+        self.distance_noise = 0.0 # initialize distance noise to zero
+
+    # --------
+    # set: 
+    #    sets a robot coordinate
+    #
+
+    def set(self, new_x, new_y, new_orientation):
+
+        if new_orientation < 0 or new_orientation >= 2 * pi:
+            raise ValueError, 'Orientation must be in [0..2pi]'
+        self.x = float(new_x)
+        self.y = float(new_y)
+        self.orientation = float(new_orientation)
+
+    # --------
+    # set_noise: 
+    #    sets the noise parameters
+    #
+    def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
+        # makes it possible to change the noise parameters
+        # this is often useful in particle filters
+        self.bearing_noise  = float(new_b_noise)
+        self.steering_noise = float(new_s_noise)
+        self.distance_noise = float(new_d_noise)
+
+    # --------
+    # measurement_prob
+    #    computes the probability of a measurement
+    #  
+
+    def measurement_prob(self, measurements):
+
+        # calculate the correct measurement
+        predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise.
+
+
+        # compute errors
+        error = 1.0
+        for i in range(len(measurements)):
+            error_bearing = abs(measurements[i] - predicted_measurements[i])
+            error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate
+            
+
+            # update Gaussian
+            error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) /  
+                      sqrt(2.0 * pi * (self.bearing_noise ** 2)))
+
+        return error
+    
+    def __repr__(self): #allows us to print robot attributes.
+        return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), 
+                                                str(self.orientation))
+    
+    ############# ONLY ADD/MODIFY CODE BELOW HERE ###################
+       
+    # --------
+    # move: 
+    #   
+    # copy your code from the previous exercise
+    # and modify it so that it simulates motion noise
+    # according to the noise parameters
+    #           self.steering_noise
+    #           self.distance_noise
+
+    def move(self, motion): 
+        epsilon = 0.001
+        L = self.length
+        alpha, d = motion #angle and length of motion
+        #adding noises:
+        alpha += random.gauss(0.0, self.steering_noise)
+        d += random.gauss(0.0, self.distance_noise)
+        theta  = self.orientation
+
+        beta = d/L * tan(alpha)
+        
+        if abs(beta) < epsilon:
+            new_x = self.x + cos(theta) * d
+            new_y = self.y + sin(theta) * d
+        else:
+            R = d/beta #turning radius
+            cx = self.x - sin(theta) * R
+            cy = self.y + cos(theta) * R
+            new_x = cx + sin(theta + beta) * R
+            new_y = cy - cos(theta + beta) * R
+
+        new_theta = (theta + beta) % (2*pi)
+
+        result = robot(self.length)
+        result.set(new_x, new_y, new_theta)
+        result.set_noise(self.bearing_noise, self.steering_noise,
+                         self.distance_noise)
+        
+        return result
+    
+    # --------
+    # sense: 
+    #    
+
+    # copy your code from the previous exercise
+    # and modify it so that it simulates bearing noise
+    # according to
+    #           self.bearing_noise
+    def sense(self, has_noise = 1):
+        Z = []
+
+        # ENTER CODE HERE
+        # HINT: You will probably need to use the function atan2()
+        for point in landmarks:
+            dy = point[0] - self.y
+            dx = point[1] - self.x
+            beta = atan2(dy, dx) % (2*pi)
+            if has_noise:
+                beta += random.gauss(0.0, self.bearing_noise)
+            Z.append(beta - self.orientation)
+        
+        return Z 
+    
+
+    ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
+
+# --------
+#
+# extract position from a particle set
+# 
+
+def get_position(p):
+    x = 0.0
+    y = 0.0
+    orientation = 0.0
+    for i in range(len(p)):
+        x += p[i].x
+        y += p[i].y
+        # orientation is tricky because it is cyclic. By normalizing
+        # around the first particle we are somewhat more robust to
+        # the 0=2pi problem
+        orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi)) 
+                        + p[0].orientation - pi)
+    return [x / len(p), y / len(p), orientation / len(p)]
+
+# --------
+#
+# The following code generates the measurements vector
+# You can use it to develop your solution.
+# 
+
+
+def generate_ground_truth(motions):
+
+    myrobot = robot()
+    myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
+
+    Z = []
+    T = len(motions)
+
+    for t in range(T):
+        myrobot = myrobot.move(motions[t])
+        Z.append(myrobot.sense())
+    #print 'Robot:    ', myrobot
+    return [myrobot, Z]
+
+# --------
+#
+# The following code prints the measurements associated
+# with generate_ground_truth
+#
+
+def print_measurements(Z):
+
+    T = len(Z)
+
+    print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \
+        (str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3]))
+    for t in range(1,T-1):
+        print '                [%.8s, %.8s, %.8s, %.8s],' % \
+            (str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3]))
+    print '                [%.8s, %.8s, %.8s, %.8s]]' % \
+        (str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3]))
+
+# --------
+#
+# The following code checks to see if your particle filter
+# localizes the robot to within the desired tolerances
+# of the true position. The tolerances are defined at the top.
+#
+
+def check_output(final_robot, estimated_position):
+
+    error_x = abs(final_robot.x - estimated_position[0])
+    error_y = abs(final_robot.y - estimated_position[1])
+    error_orientation = abs(final_robot.orientation - estimated_position[2])
+    error_orientation = (error_orientation + pi) % (2.0 * pi) - pi
+    correct = error_x < tolerance_xy and error_y < tolerance_xy \
+              and error_orientation < tolerance_orientation
+    return correct
+
+
+
+def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!
+    # --------
+    #
+    # Make particles
+    # 
+
+    p = []
+    for i in range(N):
+        r = robot()
+        r.set_noise(bearing_noise, steering_noise, distance_noise)
+        p.append(r)
+
+    # --------
+    #
+    # Update particles
+    #     
+
+    for t in range(len(motions)):
+    
+        # motion update (prediction)
+        p2 = []
+        for i in range(N):
+            p2.append(p[i].move(motions[t]))
+        p = p2
+
+        # measurement update
+        w = []
+        for i in range(N):
+            w.append(p[i].measurement_prob(measurements[t]))
+
+        # resampling
+        p3 = []
+        index = int(random.random() * N)
+        beta = 0.0
+        mw = max(w)
+        for i in range(N):
+            beta += random.random() * 2.0 * mw
+            while beta > w[index]:
+                beta -= w[index]
+                index = (index + 1) % N
+            p3.append(p[index])
+        p = p3
+    
+    return get_position(p)
+
+## IMPORTANT: You may uncomment the test cases below to test your code.
+## But when you submit this code, your test cases MUST be commented
+## out.
+##
+## You can test whether your particle filter works using the
+## function check_output (see test case 2). We will be using a similar
+## function. Note: Even for a well-implemented particle filter this
+## function occasionally returns False. This is because a particle
+## filter is a randomized algorithm. We will be testing your code
+## multiple times. Make sure check_output returns True at least 80%
+## of the time.
+
+
+ 
+## --------
+## TEST CASES:
+## 
+##1) Calling the particle_filter function with the following
+##    motions and measurements should return a [x,y,orientation]
+##    vector near [x=93.476 y=75.186 orient=5.2664], that is, the
+##    robot's true location.
+##
+##motions = [[2. * pi / 10, 20.] for row in range(8)]
+##measurements = [[4.746936, 3.859782, 3.045217, 2.045506],
+##                [3.510067, 2.916300, 2.146394, 1.598332],
+##                [2.972469, 2.407489, 1.588474, 1.611094],
+##                [1.906178, 1.193329, 0.619356, 0.807930],
+##                [1.352825, 0.662233, 0.144927, 0.799090],
+##                [0.856150, 0.214590, 5.651497, 1.062401],
+##                [0.194460, 5.660382, 4.761072, 2.471682],
+##                [5.717342, 4.736780, 3.909599, 2.342536]]
+##
+##print particle_filter(motions, measurements)
+
+## 2) You can generate your own test cases by generating
+##    measurements using the generate_ground_truth function.
+##    It will print the robot's last location when calling it.
+##
+##
+##number_of_iterations = 6
+##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]
+##
+##x = generate_ground_truth(motions)
+##final_robot = x[0]
+##measurements = x[1]
+##estimated_position = particle_filter(motions, measurements)
+##print_measurements(measurements)
+##print 'Ground truth:    ', final_robot
+##print 'Particle filter: ', estimated_position
+##print 'Code check:      ', check_output(final_robot, estimated_position)
+from Tkinter import *
+from hw3_6 import *
+
+from time import sleep
+
+#Developped with Python 2.7.2
+#
+#   How to use it :
+# 1)  Copy and save this file
+# 2)  Copy your code in homework 3-6 and
+#save it as hw3_6_ParticleFilter.py in the
+#same directory
+# 3)  Execute this file
+
+#***************************************
+class DispParticleFilter(Tk):
+
+    '''frameLength is the delay between two frames, i.e. two steps of the filter'''
+    def __init__(self, motions, N=500, frameLength = 0.5, displayRealRobot = True, displayGhost = False ):
+        Tk.__init__(self)
+        self.title( 'Diplay Particle Filter CS373-HW03.06')
+        self.motions = motions        
+        self.N = N
+
+        self.frameLength = frameLength
+        self.displayRealRobot = displayRealRobot        
+        self.displayGhost = displayGhost
+        #init particle filter
+        self.initFilter()
+        # Drawing
+        self.margin = 100                                    # margin
+        self.zoom_factor = 2                                # zoom factor
+        self.playing = False
+        self.can = DisplayParticles ( self.margin, self.zoom_factor )
+        self.can.configure(bg ='ivory', bd =2, relief=SUNKEN)
+        self.can.pack(side =TOP, padx =5, pady =5)
+        self.can.draw_all(self.p, self.robot, self.displayRealRobot, self.displayGhost)
+        #Buttons
+        self.buttonReset = Button(self, text ='Reset', command =self.resetFilter)
+        self.buttonReset.pack(side =LEFT, padx =5, pady =5)
+        self.buttonNext = Button(self, text ='Next step', command =self.nextStep)
+        self.buttonNext.pack(side =LEFT, padx =5, pady =5)
+        self.buttonPlay = Button(self, text ='Play', command =self.play)
+        self.buttonPlay.pack(side =LEFT, padx =5, pady =5)
+        self.buttonPause = Button(self, text ='Pause', command =self.pause)
+        self.buttonPause.pack(side =LEFT, padx =5, pady =5)    
+        self.buttonPause.configure(state=DISABLED)         
+        #Label
+        textLabel = 'Current state = ' + str(self.actualState+1) + '/' + str(len(motions))
+        self.label = Label(self, text = textLabel )
+        self.label.pack(side = BOTTOM, padx =5, pady =5)
+
+    def resetFilter(self):
+        self.pause()
+
+        self.initFilter()
+        #Replot all
+        self.can.draw_all(self.p, self.robot, self.displayRealRobot, self.displayGhost)
+
+    def initFilter (self):
+
+        #New Robot's position
+        self.robot = robot()
+        self.robot.set_noise(bearing_noise, steering_noise, distance_noise)
+
+        # Make particles            
+        self.p = []                     # p : particles set
+        for i in range(self.N):
+            r = robot()
+            r.set_noise(bearing_noise, steering_noise, distance_noise)
+            self.p.append(r)
+        # --------------
+        self.actualState = 0
+
+    def nextStep (self, event=None):
+        self.actualState = self.actualState + 1
+        if self.actualState < len(self.motions):
+            #Label
+            stateString = 'Actual state = ' + str(self.actualState+1) + '/' + str(len(motions))
+            self.label.configure( text = stateString )
+            # motion update (prediction)
+            self.robot = self.robot.move(self.motions[self.actualState])
+            p2 = []
+            for i in range(self.N):
+                p2.append(self.p[i].move(self.motions[self.actualState]))
+            self.p = p2
+            # measurement update
+            w = []
+            for i in range(self.N):
+                w.append(self.p[i].measurement_prob( self.robot.sense() ))
+            # resampling
+            p3 = []
+            index = int(random.random() * self.N)
+            beta = 0.0
+            mw = max(w)
+            for i in range(self.N):
+                beta += random.random() * 2.0 * mw
+                while beta > w[index]:
+                    beta -= w[index]
+                    index = (index + 1) % self.N
+                p3.append(self.p[index])
+            self.p = p3
+            #Replot all
+            self.can.draw_all(self.p, self.robot, self.displayRealRobot, self.displayGhost)
+            return True
+        else:
+            return False
+
+    def play (self, event=None):
+        self.playing = True
+        self.buttonPause.configure(state=NORMAL)  
+        self.buttonNext.configure(state=DISABLED) 
+        self.buttonPlay.configure(state=DISABLED) 
+        while self.playing:
+            if self.nextStep() == False:
+                self.pause(event)
+                self.buttonPlay.configure(state=DISABLED)  
+                self.buttonNext.configure(state=DISABLED)                 
+                break
+            self.update()
+            sleep(self.frameLength)
+
+    def pause (self, event=None):
+        self.playing = False
+        self.buttonPause.configure(state=DISABLED)  
+        self.buttonNext.configure(state=NORMAL) 
+        self.buttonPlay.configure(state=NORMAL)
+
+class DisplayParticles(Canvas):
+
+    def __init__(self, margin, zoom_factor ):
+        Canvas.__init__(self)
+        #self.p = p
+        self.margin = margin
+        self.zoom_factor = zoom_factor
+        self.larg = (2*margin + world_size) * zoom_factor
+        self.haut = self.larg
+        self.configure(width=self.larg, height=self.haut )
+        self.larg, self.haut = (2*margin + world_size) * zoom_factor, (2*margin + world_size) * zoom_factor
+        # Landmarks
+        self.landmarks_radius = 2
+        self.landmarks_color = 'green'
+        # Particles
+        self.particle_radius = 1
+        self.particle_color = 'red'
+        # Robot
+        self.robot_radius = 4
+        self.robot_color = 'blue'
+        self.ghost_color = None
+
+    def draw_all(self, p, realRob, displayRealRobot, displayGhost):
+        #print len(p)
+        self.configure(bg ='ivory', bd =2, relief=SUNKEN)
+        self.delete(ALL)
+        self.p = p
+        self.plot_particles()
+
+        if displayGhost:
+            ghost = get_position(self.p)
+            self.plot_robot( ghost[0], ghost[1], ghost[2], self.robot_radius, self.ghost_color)
+        self.plot_landmarks( landmarks, self.landmarks_radius, self.landmarks_color )
+
+        if displayRealRobot:
+            self.plot_robot( realRob.x, realRob.y, realRob.orientation, self.robot_radius, self.robot_color)
+
+    def plot_landmarks(self, lms, radius, l_color ):
+        for lm in lms:
+            x0 = (self.margin + lm[1] - radius) * self.zoom_factor
+            y0 = (self.margin + lm[0] - radius) * self.zoom_factor
+            x1 = (self.margin + lm[1] + radius) * self.zoom_factor
+            y1 = (self.margin + lm[0] + radius) * self.zoom_factor
+            self.create_oval( x0, y0, x1, y1, fill = l_color )
+
+    def plot_particles(self):
+        for particle in self.p:
+            self.draw_particle( particle, self.particle_radius, self.particle_color )
+
+    def draw_particle(self, particle, radius, p_color):
+        #x0 = (self.margin + particle.x - radius) * self.zoom_factor
+        #y0 = (self.margin + particle.y - radius) * self.zoom_factor
+        #x1 = (self.margin + particle.x + radius) * self.zoom_factor
+        #y1 = (self.margin + particle.y + radius) * self.zoom_factor
+        #self.create_oval( x0, y0, x1, y1, fill = p_color )
+        x2 = (self.margin + particle.x) * self.zoom_factor
+        y2 = (self.margin + particle.y) * self.zoom_factor
+        x3 = (self.margin + particle.x + 2*radius*cos(particle.orientation)) * self.zoom_factor
+        y3 = (self.margin + particle.y + 2*radius*sin(particle.orientation)) * self.zoom_factor
+        self.create_line( x2, y2, x3, y3, fill = p_color, width =self.zoom_factor,
+                          arrow=LAST, arrowshape=(2*self.zoom_factor,
+                                                  3*self.zoom_factor,
+                                                  1*self.zoom_factor) )
+
+    def plot_robot(self, x,y, orientation, radius, r_color):
+        x0 = (self.margin + x - radius) * self.zoom_factor
+        y0 = (self.margin + y - radius) * self.zoom_factor
+        x1 = (self.margin + x + radius) * self.zoom_factor
+        y1 = (self.margin + y + radius) * self.zoom_factor
+        self.create_oval( x0, y0, x1, y1, fill = r_color )
+        x2 = (self.margin + x) * self.zoom_factor
+        y2 = (self.margin + y) * self.zoom_factor
+        x3 = (self.margin + x + 2*radius*cos(orientation)) * self.zoom_factor
+        y3 = (self.margin + y + 2*radius*sin(orientation)) * self.zoom_factor
+        self.create_line( x2, y2, x3, y3, fill = r_color, width =self.zoom_factor, arrow=LAST )
+
+#**************************************************
+
+if __name__ == "__main__":
+    #motions  ( here copy of the dataset in hw3-6 )
+    number_of_iterations = 20
+    motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]
+
+    #Display window
+    wind = DispParticleFilter ( motions, 500, displayRealRobot = True, displayGhost = False )
+    wind.mainloop()
+# --------------
+# USER INSTRUCTIONS
+#
+# Write a function called stochastic_value that
+# takes no input and RETURNS two grids. The
+# first grid, value, should contain the computed
+# value of each cell as shown in the video. The
+# second grid, policy, should contain the optimum
+# policy for each cell.
+#
+# Stay tuned for a homework help video! This should
+# be available by Thursday and will be visible
+# in the course content tab.
+#
+# Good luck! Keep learning!
+#
+# --------------
+# GRADING NOTES
+#
+# We will be calling your stochastic_value function
+# with several different grids and different values
+# of success_prob, collision_cost, and cost_step.
+# In order to be marked correct, your function must
+# RETURN (it does not have to print) two grids,
+# value and policy.
+#
+# When grading your value grid, we will compare the
+# value of each cell with the true value according
+# to this model. If your answer for each cell
+# is sufficiently close to the correct answer
+# (within 0.001), you will be marked as correct.
+#
+# NOTE: Please do not modify the values of grid,
+# success_prob, collision_cost, or cost_step inside
+# your function. Doing so could result in your
+# submission being inappropriately marked as incorrect.
+
+# -------------
+# GLOBAL VARIABLES
+#
+# You may modify these variables for testing
+# purposes, but you should only modify them here.
+# Do NOT modify them inside your stochastic_value
+# function.
+
+grid = [[0, 0, 0, 0],
+        [0, 0, 0, 0],
+        [0, 1, 0, 0],
+        [0, 1, 1, 0]]
+
+goal = [0, len(grid[0])-1] # Goal is in top right corner
+
+delta = [[-1, 0 ], # go up
+         [ 0, -1], # go left
+         [ 1, 0 ], # go down
+         [ 0, 1 ]] # go right
+
+delta_name = ['^', '<', 'v', '>'] # Use these when creating your policy grid.
+
+success_prob = 0.5
+failure_prob = (1.0 - success_prob)/2.0 # Probability(stepping left) = prob(stepping right) = failure_prob
+collision_cost = 100
+cost_step = 1
+
+############## INSERT/MODIFY YOUR CODE BELOW ##################
+#
+# You may modify the code below if you want, but remember that
+# your function must...
+#
+# 1) ...be called stochastic_value().
+# 2) ...NOT take any arguments.
+# 3) ...return two grids: FIRST value and THEN policy.
+
+def stochastic_value():
+    enourmous_number = 1000
+    value = [[enourmous_number for row in range(len(grid[0]))] for col in range(len(grid))]
+    policy = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
+
+    def get_val(row,col):
+        if 0 <= row < len(grid) and 0 <= col < len(grid[0])\
+                and grid[row][col] == 0:
+            return  value[row][col]
+        else:
+            return collision_cost
+
+    def get_sides(a_id):
+        '''returns ids of deltas, where robot may slide '''
+        if 0 <= a_id < len(delta):
+            n = len(delta)
+            val1 = (a_id + 1) % n
+            val2 = (a_id + 3) % n
+            return val1, val2
+        else:
+            return None
+
+
+    change = True
+    while change:
+        change = False
+
+        for row in range(len(grid)):
+            for col in range(len(grid[0])):
+                if goal == [row, col] and value[row][col] > 0:
+                    value[row][col] = 0
+                    policy[row][col] = '*'
+
+                elif grid[row][col] == 0:
+                    rounds = []
+
+                    for a_id, action in enumerate(delta):
+                        slides = get_sides(a_id)
+                        x2 = row + delta[a_id][0]
+                        y2 = col + delta[a_id][1]
+
+                        x3 = row + delta[slides[0]][0]
+                        y3 = col + delta[slides[0]][1]
+                        x4 = row + delta[slides[1]][0]
+                        y4 = col + delta[slides[1]][1]
+                        val = success_prob * get_val(x2, y2) + cost_step
+                        val += failure_prob * (get_val(x3, y3) + get_val(x4, y4))
+                        rounds.append(val)
+
+
+                    min_val = min(rounds)
+                    if min_val < value[row][col]:
+                        value[row][col] = min_val
+                        policy[row][col] = delta_name[rounds.index(min_val)]
+                        change = True
+
+    for row in value:
+        print row
+
+    for row in policy:
+        print row
+    return value, policy
+
+stochastic_value()
+
+
+# -------------
+# User Instructions
+#
+# Now you will be incorporating fixed points into
+# your smoother. 
+#
+# You will need to use the equations from gradient
+# descent AND the new equations presented in the
+# previous lecture to implement smoothing with
+# fixed points.
+#
+# Your function should return the newpath that it
+# calculates. 
+#
+# Feel free to use the provided solution_check function
+# to test your code. You can find it at the bottom.
+#
+# --------------
+# Testing Instructions
+# 
+# To test your code, call the solution_check function with
+# two arguments. The first argument should be the result of your
+# smooth function. The second should be the corresponding answer.
+# For example, calling
+#
+# solution_check(smooth(testpath1), answer1)
+#
+# should return True if your answer is correct and False if
+# it is not.
+
+from math import *
+
+# Do not modify path inside your function.
+path=[[0, 0], #fix 
+      [1, 0],
+      [2, 0],
+      [3, 0],
+      [4, 0],
+      [5, 0],
+      [6, 0], #fix
+      [6, 1],
+      [6, 2],
+      [6, 3], #fix
+      [5, 3],
+      [4, 3],
+      [3, 3],
+      [2, 3],
+      [1, 3],
+      [0, 3], #fix
+      [0, 2],
+      [0, 1]]
+
+# Do not modify fix inside your function
+fix = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0]
+
+######################## ENTER CODE BELOW HERE #########################
+
+def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.001):
+    #
+    # Enter code here. 
+    # The weight for each of the two new equations should be 0.5 * weight_smooth
+    #
+# Enter code here
+    newpath = [[float(col) for col in row] for row in path]
+    change = tolerance
+    while change >= tolerance:
+        change = 0.0
+        for i in range(len(path)):
+            if fix[i]:
+                continue
+            #else update coordinates
+            for j in range(len(path[0])):
+                aux = newpath[i][j]
+                x = path[i][j]
+                xn1 = (i+1) % len(path)
+                xn2 = (i+2) % len(path)
+                xp1 = (i-1) % len(path)
+                xp2 = (i-2) % len(path)
+                
+                newpath[i][j] += weight_data * (x - newpath[i][j])
+                newpath[i][j] += weight_smooth * (
+                    newpath[xn1][j] + newpath[xp1][j] - 2.0*newpath[i][j])
+                newpath[i][j] += 0.5 * weight_smooth * (
+                    2.0* newpath[xp1][j] - newpath[xp2][j] - newpath[i][j])
+                newpath[i][j] += 0.5 * weight_smooth * (
+                    2.0 * newpath[xn1][j] - newpath[xn2][j] - newpath[i][j])
+                change += abs(aux - newpath[i][j])
+    
+    return newpath
+
+#thank you - EnTerr - for posting this on our discussion forum
+
+##newpath = smooth(path)
+##for i in range(len(path)):
+##    print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']'
+
+# --------------------------------------------------
+# check if two numbers are 'close enough,'used in
+# solution_check function.
+#
+def close_enough(user_answer, true_answer, epsilon = 0.03):
+    if abs(user_answer - true_answer) > epsilon:
+        return False
+    return True
+
+# --------------------------------------------------
+# check your solution against our reference solution for
+# a variety of test cases (given below)
+#
+def solution_check(newpath, answer):
+    if type(newpath) != type(answer):
+        print "Error. You do not return a list."
+        return False
+    if len(newpath) != len(answer):
+        print 'Error. Your newpath is not the correct length.'
+        return False
+    if len(newpath[0]) != len(answer[0]):
+        print 'Error. Your entries do not contain an (x, y) coordinate pair.'
+        return False
+    for i in range(len(newpath)): 
+        for j in range(len(newpath[0])):
+            if not close_enough(newpath[i][j], answer[i][j]):
+                print 'Error, at least one of your entries is not correct.'
+                return False
+    print "Test case correct!"
+    return True
+
+# --------------
+# Testing Instructions
+# 
+# To test your code, call the solution_check function with
+# two arguments. The first argument should be the result of your
+# smooth function. The second should be the corresponding answer.
+# For example, calling
+#
+# solution_check(smooth(testpath1), answer1)
+#
+# should return True if your answer is correct and False if
+# it is not.
+
+testpath1=[[0, 0], #fix
+      [1, 0],
+      [2, 0],
+      [3, 0],
+      [4, 0],
+      [5, 0],
+      [6, 0], #fix
+      [6, 1],
+      [6, 2],
+      [6, 3], #fix
+      [5, 3],
+      [4, 3],
+      [3, 3],
+      [2, 3],
+      [1, 3],
+      [0, 3], #fix
+      [0, 2],
+      [0, 1]]
+testfix1 = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0]
+answer1 = [[0, 0],
+           [0.7938620981547201, -0.8311168821106101],
+           [1.8579052986461084, -1.3834788165869276],
+           [3.053905318597796, -1.5745863173084],
+           [4.23141390533387, -1.3784271816058231],
+           [5.250184859723701, -0.8264215958231558],
+           [6, 0],
+           [6.415150091996651, 0.9836951698796843],
+           [6.41942442687092, 2.019512290770163],
+           [6, 3],
+           [5.206131365604606, 3.831104483245191],
+           [4.142082497497067, 4.383455704596517],
+           [2.9460804122779813, 4.5745592975708105],
+           [1.768574219397359, 4.378404668718541],
+           [0.7498089205417316, 3.826409771585794],
+           [0, 3],
+           [-0.4151464728194156, 2.016311854977891],
+           [-0.4194207879552198, 0.9804948340550833]]
+
+testpath2 = [[0, 0], # fix
+             [2, 0],
+             [4, 0], # fix
+             [4, 2],
+             [4, 4], # fix
+             [2, 4],
+             [0, 4], # fix
+             [0, 2]]
+testfix2 = [1, 0, 1, 0, 1, 0, 1, 0]
+answer2 = [[0, 0],
+           [2.0116767115496095, -0.7015439080661671],
+           [4, 0],
+           [4.701543905420104, 2.0116768147460418],
+           [4, 4],
+           [1.9883231877640861, 4.701543807525115],
+           [0, 4],
+           [-0.7015438099112995, 1.9883232808252207]]
+
+print smooth(testpath2, testfix2)
+solution_check(smooth(testpath2, testfix2), answer2)
+
+
+# --------------
+# User Instructions
+# 
+# Define a function cte in the robot class that will
+# compute the crosstrack error for a robot on a
+# racetrack with a shape as described in the video.
+#
+# You will need to base your error calculation on
+# the robot's location on the track. Remember that 
+# the robot will be traveling to the right on the
+# upper straight segment and to the left on the lower
+# straight segment.
+#
+# --------------
+# Grading Notes
+#
+# We will be testing your cte function directly by
+# calling it with different robot locations and making
+# sure that it returns the correct crosstrack error.  
+ 
+from math import *
+import random
+
+
+# ------------------------------------------------
+# 
+# this is the robot class
+#
+
+class robot:
+
+    # --------
+    # init: 
+    #    creates robot and initializes location/orientation to 0, 0, 0
+    #
+
+    def __init__(self, length = 20.0):
+        self.x = 0.0
+        self.y = 0.0
+        self.orientation = 0.0
+        self.length = length
+        self.steering_noise = 0.0
+        self.distance_noise = 0.0
+        self.steering_drift = 0.0
+
+    # --------
+    # set: 
+    #	sets a robot coordinate
+    #
+
+    def set(self, new_x, new_y, new_orientation):
+
+        self.x = float(new_x)
+        self.y = float(new_y)
+        self.orientation = float(new_orientation) % (2.0 * pi)
+
+
+    # --------
+    # set_noise: 
+    #	sets the noise parameters
+    #
+
+    def set_noise(self, new_s_noise, new_d_noise):
+        # makes it possible to change the noise parameters
+        # this is often useful in particle filters
+        self.steering_noise = float(new_s_noise)
+        self.distance_noise = float(new_d_noise)
+
+    # --------
+    # set_steering_drift: 
+    #	sets the systematical steering drift parameter
+    #
+
+    def set_steering_drift(self, drift):
+        self.steering_drift = drift
+        
+    # --------
+    # move: 
+    #    steering = front wheel steering angle, limited by max_steering_angle
+    #    distance = total distance driven, most be non-negative
+
+    def move(self, steering, distance, 
+             tolerance = 0.001, max_steering_angle = pi / 4.0):
+
+        if steering > max_steering_angle:
+            steering = max_steering_angle
+        if steering < -max_steering_angle:
+            steering = -max_steering_angle
+        if distance < 0.0:
+            distance = 0.0
+
+
+        # make a new copy
+        res = robot()
+        res.length         = self.length
+        res.steering_noise = self.steering_noise
+        res.distance_noise = self.distance_noise
+        res.steering_drift = self.steering_drift
+
+        # apply noise
+        steering2 = random.gauss(steering, self.steering_noise)
+        distance2 = random.gauss(distance, self.distance_noise)
+
+        # apply steering drift
+        steering2 += self.steering_drift
+
+        # Execute motion
+        turn = tan(steering2) * distance2 / res.length
+
+        if abs(turn) < tolerance:
+
+            # approximate by straight line motion
+
+            res.x = self.x + (distance2 * cos(self.orientation))
+            res.y = self.y + (distance2 * sin(self.orientation))
+            res.orientation = (self.orientation + turn) % (2.0 * pi)
+
+        else:
+
+            # approximate bicycle model for motion
+
+            radius = distance2 / turn
+            cx = self.x - (sin(self.orientation) * radius)
+            cy = self.y + (cos(self.orientation) * radius)
+            res.orientation = (self.orientation + turn) % (2.0 * pi)
+            res.x = cx + (sin(res.orientation) * radius)
+            res.y = cy - (cos(res.orientation) * radius)
+
+        return res
+
+
+
+
+    def __repr__(self):
+        return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)
+
+
+############## ONLY ADD / MODIFY CODE BELOW THIS LINE ####################
+   
+    def cte(self, radius):
+        def calc_distance(pt1, pt2):
+            return sqrt(sum(map(lambda (x,y): (x-y)**2, zip(pt1, pt2))))
+        
+        R = float(radius)
+        cte = 0.0
+        pt = [0., 0.]
+        mark = 1.0
+        if self.x <= R:
+            #1.half-circle
+            pt = [R, R]
+            cte = calc_distance(pt, [self.x, self.y]) - R
+        elif self.x >= 3*R:
+            #2.half-circle
+            pt = [3*R, R]
+            cte = calc_distance(pt, [self.x, self.y]) - R
+        elif self.y > R:
+            #upper line
+            pt = [self.x, 2*R]
+            mark = 1.0 if self.y > pt[1] else -1.0
+            cte = mark * calc_distance(pt, [self.x, self.y])
+        elif self.y <= R:
+            #bottom line
+            pt = [self.x, 0.]
+            mark = 1.0 if self.y <= pt[1] else -1.0
+            cte = mark * calc_distance(pt, [self.x, self.y])
+        else:
+            print "cte doesnt match coordinates."
+
+        
+        return cte
+    
+############## ONLY ADD / MODIFY CODE ABOVE THIS LINE ####################
+
+
+
+
+# ------------------------------------------------------------------------
+#
+# run - does a single control run.
+
+carhistory = []
+def run(params, radius, printflag = False):
+    myrobot = robot()
+    myrobot.set(0.0, radius, pi / 2.0)
+    speed = 1.0 # motion distance is equal to speed (we assume time = 1)
+    err = 0.0
+    int_crosstrack_error = 0.0
+    N = 200
+
+    crosstrack_error = myrobot.cte(radius) # You need to define the cte function!
+    
+    for i in range(N*2):
+        carhistory.append((myrobot.x, myrobot.y))
+
+        diff_crosstrack_error = - crosstrack_error
+        crosstrack_error = myrobot.cte(radius)
+        diff_crosstrack_error += crosstrack_error
+        int_crosstrack_error += crosstrack_error
+        steer = - params[0] * crosstrack_error \
+                - params[1] * diff_crosstrack_error \
+                - params[2] * int_crosstrack_error
+        myrobot = myrobot.move(steer, speed)
+        if i >= N:
+            err += crosstrack_error ** 2
+        if printflag:
+            print myrobot, crosstrack_error
+    return err / float(N)
+
+radius = 25.0
+params = [10.0, 15.0, 0]
+err = run(params, radius, True)
+print '\nFinal paramaeters: ', params, '\n ->', err
+
+def generate_track(radius):
+    track = []
+
+    # Upper left section
+    for a in range(180, 90, -1):
+        x = radius * cos(float(a)*(2*pi/360.0)) + radius
+        y = radius * sin(float(a)*(2*pi/360.0)) + radius
+        track.append([x, y])
+
+    # Upper straight
+    for x in range(int(radius), int(3.0*radius)):
+        track.append([x, 2.0*radius])
+
+    # Rigth section
+    for a in range(90, -90, -1):
+        x = radius * cos(float(a)*(2*pi/360.0)) + 3.0 * radius
+        y = radius * sin(float(a)*(2*pi/360.0)) + radius
+        track.append([x, y])
+
+    # Lower straight
+    for x in range(int(3.0*radius), int(radius), -1):
+        track.append([x, 0.0])
+
+    # Lower left section
+    for a in range(270, 180, -1):
+        x = radius * cos(float(a)*(2*pi/360.0)) + radius
+        y = radius * sin(float(a)*(2*pi/360.0)) + radius
+        track.append([x, y])
+
+    return track
+
+
+import matplotlib.pyplot as mtpl
+mtpl.figure()
+mtpl.hold(True)
+mtpl.plot([p[0] for p in carhistory], [p[1] for p in carhistory], label='Car path')
+track = generate_track(radius)
+mtpl.plot([p[0] for p in track], [p[1] for p in track], label='Track')
+
+mtpl.legend()
+mtpl.show()

unit1/u1-12-sensefunc.py

+#Modify the code below so that the function sense, which 
+#takes p and Z as inputs, will output the NON-normalized 
+#probability distribution, q, after multiplying the entries 
+#in p by pHit or pMiss according to the color in the 
+#corresponding cell in world.
+
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+Z = 'red'
+pHit = 0.6
+pMiss = 0.2
+
+def sense(p, Z):
+    #
+    #ADD YOUR CODE HERE
+	#
+    q = [0.0 for i in p]
+    for i, state in enumerate(world):
+        if state == "red":
+            q[i] = pHit * p[i]
+        elif state == "green":
+            q[i] = pMiss * p[i]
+        else:
+            print "Unknown state of world: ", state
+    
+    return q
+
+print sense(p,Z)

unit1/u1-13-normed-sensefunc.py

+#Modify your code so that it normalizes the output for 
+#the function sense. This means that the entries in q 
+#should sum to one.
+
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+Z = 'red'
+pHit = 0.6
+pMiss = 0.2
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+        
+    norm = sum(q)
+    q = [item/norm for item in q]
+    return q
+print sense(p,Z)

unit1/u1-14-testsense.py

+#Try using your code with a measurement of 'green' and 
+#make sure the resulting probability distribution is correct.
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+Z = 'green'
+pHit = 0.6
+pMiss = 0.2
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(p)):
+        q[i]=q[i]/s 
+    return q
+print sense(p, Z)

unit1/u1-15-multmeasurments.py

+#Modify the code so that it updates the probability twice
+#and gives the posterior distribution after both 
+#measurements are incorporated. Make sure that your code 
+#allows for any sequence of measurement of any length.
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+pHit = 0.6
+pMiss = 0.2
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+#
+#ADD YOUR CODE HERE
+for Z in measurements:
+    #update probability
+    p = sense(p, Z)
+    #p = [p[i] * q[i] for i in range(0, len(p))]
+
+print p

unit1/u1-17-movefunc.py

+#Program a function that returns a new distribution 
+#q, shifted to the right by U units. If U=0, q should 
+#be the same as p.
+
+p=[0, 1, 0, 0, 0]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+pHit = 0.6
+pMiss = 0.2
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    #
+    #ADD CODE HERE
+    #
+    q = p[U:] + p[:U]
+    return q
+
+print move(p, 1)

unit1/u1-21_inexact_motion.py

+#Modify the move function to accommodate the added 
+#probabilities of overshooting or undershooting 
+#the intended destination.
+
+p=[0, 1, 0, 0, 0]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    q = [0.0 for i in p]
+    for i in range(len(p)):
+        q[(i+U-1)% len(p)] += p[i] * pUndershoot
+        q[(i+U)% len(p)]   += p[i] * pExact
+        q[(i+U+1)% len(p)] += p[i] * pOvershoot
+    return q
+    
+
+print move(p, 1)

unit1/u1-23_movetwice.py

+#Write code that makes the robot move twice and then prints 
+#out the resulting distribution, starting with the initial 
+#distribution p = [0, 1, 0, 0, 0]
+
+p=[0, 1, 0, 0, 0]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    q = []
+    for i in range(len(p)):
+        s = pExact * p[(i-U) % len(p)]
+        s = s + pOvershoot * p[(i-U-1) % len(p)]
+        s = s + pUndershoot * p[(i-U+1) % len(p)]
+        q.append(s)
+    return q
+#
+# ADD CODE HERE
+#
+steps= 2
+for i in range(steps):
+    p = move(p,1)
+    #p = map(sum, zip(p, p2))
+    
+print p

unit1/u1-24_move1000.py

+#write code that moves 1000 times and then prints the 
+#resulting probability distribution.
+
+p=[0, 1, 0, 0, 0]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    q = []
+    for i in range(len(p)):
+        s = pExact * p[(i-U) % len(p)]
+        s = s + pOvershoot * p[(i-U-1) % len(p)]
+        s = s + pUndershoot * p[(i-U+1) % len(p)]
+        q.append(s)
+    return q
+#
+# ADD CODE HERE
+#
+steps = 1000
+U = 1
+for i in range(steps):
+    p = move(p, U)
+
+print p

unit1/u1-25_sensemove.py

+#Given the list motions=[1,1] which means the robot 
+#moves right and then right again, compute the posterior 
+#distribution if the robot first senses red, then moves 
+#right one, then senses green, then moves right again, 
+#starting with a uniform prior distribution.
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'green']
+motions = [1,1]
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    q = []
+    for i in range(len(p)):
+        s = pExact * p[(i-U) % len(p)]
+        s = s + pOvershoot * p[(i-U-1) % len(p)]
+        s = s + pUndershoot * p[(i-U+1) % len(p)]
+        q.append(s)
+    return q
+#
+# ADD CODE HERE
+#
+
+for i in range(len(motions)):
+    U = motions[i]
+    Z = measurements[i]
+    p = sense(p, Z)
+    p = move(p, U)
+    
+print p         

unit1/u1-26_sensemove2.py

+#Modify the previous code so that the robot senses red twice.
+
+p=[0.2, 0.2, 0.2, 0.2, 0.2]
+world=['green', 'red', 'red', 'green', 'green']
+measurements = ['red', 'red']
+motions = [1,1]
+pHit = 0.6
+pMiss = 0.2
+pExact = 0.8
+pOvershoot = 0.1
+pUndershoot = 0.1
+
+def sense(p, Z):
+    q=[]
+    for i in range(len(p)):
+        hit = (Z == world[i])
+        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
+    s = sum(q)
+    for i in range(len(q)):
+        q[i] = q[i] / s
+    return q
+
+def move(p, U):
+    q = []
+    for i in range(len(p)):
+        s = pExact * p[(i-U) % len(p)]
+        s = s + pOvershoot * p[(i-U-1) % len(p)]
+        s = s + pUndershoot * p[(i-U+1) % len(p)]
+        q.append(s)
+    return q
+
+for k in range(len(measurements)):
+    p = sense(p, measurements[k])
+    p = move(p, motions[k])
+    
+print p         

unit1/u1-hw1_localization.py

+#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)
+
+
+
+

unit2/u2-13_maxGaussian.py

+#For this problem, you aren't writing any code.
+#Instead, please just change the last argument 
+#in f() to maximize the output.
+
+from math import *
+
+def f(mu, sigma2, x):
+    return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)
+
+print f(10.,4.,10.) #Change the 8. to something else!

unit2/u2-22_new_meanvar.py

+# Write a program to update your mean and variance
+# when given the mean and variance of your belief
+# and the mean and variance of your measurement.
+# This program will update the parameters of your
+# belief function.
+
+def update(mean1, var1, mean2, var2):
+    new_mean = 1.0 /(var1+var2) * (mean1 * var2 + mean2 * var1)
+    new_var = 1.0/(1.0/var1 + 1.0/var2)
+    return [new_mean, new_var]
+
+print update(10.,8.,13., 2.)

unit2/u2-24_predict.py

+# Write a program that will predict your new mean
+# and variance given the mean and variance of your 
+# prior belief and the mean and variance of your 
+# motion. 
+
+def update(mean1, var1, mean2, var2):
+    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
+    new_var = 1/(1/var1 + 1/var2)
+    return [new_mean, new_var]
+
+def predict(mean1, var1, mean2, var2):
+    new_mean = mean1 + mean2
+    new_var = var1 + var2
+    return [new_mean, new_var]
+
+print predict(10., 4., 12., 4.)

unit2/u2-25_kalman.py

+# Write a program that will iteratively update and
+# predict based on the location measurements 
+# and inferred motions shown below. 
+
+def update(mean1, var1, mean2, var2):
+    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
+    new_var = 1/(1.0/var1 + 1.0/var2)
+    return [new_mean, new_var]
+
+def predict(mean1, var1, mean2, var2):
+    new_mean = mean1 + mean2
+    new_var = var1 + var2
+    return [new_mean, new_var]
+
+measurements = [5., 6., 7., 9., 10.]
+motion = [1., 1., 2., 1., 1.]
+measurement_sig = 4.
+motion_sig = 2.
+mu = 0
+sig = 10000
+
+#Please print out ONLY the final values of the mean
+#and the variance in a list [mu, sig]. 
+
+# Insert code here
+for i in range(0, len(measurements)):
+    mu, sig = update(measurements[i], measurement_sig, mu, sig)
+    #print "measurement: ", [mu, sig]
+    mu, sig = predict(mu, sig, motion[i], motion_sig)
+    #print "prediction: ", [mu, sig]
+
+print [mu, sig]

unit2/u2-32_kalman_matrices.py

+# Write a function 'filter' that implements a multi-
+# dimensional Kalman Filter for the example given
+
+from math import *
+
+class matrix:
+    
+    # implements basic operations of a matrix class
+    
+    def __init__(self, value):
+        self.value = value
+        self.dimx = len(value)
+        self.dimy = len(value[0])
+        if value == [[]]:
+            self.dimx = 0
+    
+    def zero(self, dimx, dimy):
+        # check if valid dimensions
+        if dimx < 1 or dimy < 1:
+            raise ValueError, "Invalid size of matrix"
+        else:
+            self.dimx = dimx
+            self.dimy = dimy
+            self.value = [[0 for row in range(dimy)] for col in range(dimx)]
+    
+    def identity(self, dim):
+        # check if valid dimension
+        if dim < 1:
+            raise ValueError, "Invalid size of matrix"
+        else:
+            self.dimx = dim
+            self.dimy = dim
+            self.value = [[0 for row in range(dim)] for col in range(dim)]
+            for i in range(dim):
+                self.value[i][i] = 1
+    
+    def show(self):
+        for i in range(self.dimx):
+            print self.value[i]
+        print ' '
+    
+    def __add__(self, other):
+        # check if correct dimensions
+        if self.dimx != other.dimx or self.dimy != other.dimy:
+            raise ValueError, "Matrices must be of equal dimensions to add"
+        else:
+            # add if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, self.dimy)
+            for i in range(self.dimx):
+                for j in range(self.dimy):
+                    res.value[i][j] = self.value[i][j] + other.value[i][j]
+            return res
+    
+    def __sub__(self, other):
+        # check if correct dimensions
+        if self.dimx != other.dimx or self.dimy != other.dimy:
+            raise ValueError, "Matrices must be of equal dimensions to subtract"
+        else:
+            # subtract if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, self.dimy)
+            for i in range(self.dimx):
+                for j in range(self.dimy):
+                    res.value[i][j] = self.value[i][j] - other.value[i][j]
+            return res
+    
+    def __mul__(self, other):
+        # check if correct dimensions
+        if self.dimy != other.dimx:
+            raise ValueError, "Matrices must be m*n and n*p to multiply"
+        else:
+            # subtract if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, other.dimy)
+            for i in range(self.dimx):
+                for j in range(other.dimy):
+                    for k in range(self.dimy):
+                        res.value[i][j] += self.value[i][k] * other.value[k][j]
+            return res
+    
+    def transpose(self):
+        # compute transpose
+        res = matrix([[]])
+        res.zero(self.dimy, self.dimx)
+        for i in range(self.dimx):
+            for j in range(self.dimy):
+                res.value[j][i] = self.value[i][j]
+        return res
+    
+    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
+    
+    def Cholesky(self, ztol=1.0e-5):
+        # Computes the upper triangular Cholesky factorization of
+        # a positive definite matrix.
+        res = matrix([[]])
+        res.zero(self.dimx, self.dimx)
+        
+        for i in range(self.dimx):
+            S = sum([(res.value[k][i])**2 for k in range(i)])
+            d = self.value[i][i] - S
+            if abs(d) < ztol:
+                res.value[i][i] = 0.0
+            else:
+                if d < 0.0:
+                    raise ValueError, "Matrix not positive-definite"
+                res.value[i][i] = sqrt(d)
+            for j in range(i+1, self.dimx):
+                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
+                if abs(S) < ztol:
+                    S = 0.0
+                res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
+        return res
+    
+    def CholeskyInverse(self):
+        # Computes inverse of matrix given its Cholesky upper Triangular
+        # decomposition of matrix.
+        res = matrix([[]])
+        res.zero(self.dimx, self.dimx)
+        
+        # Backward step for inverse.
+        for j in reversed(range(self.dimx)):
+            tjj = self.value[j][j]
+            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
+            res.value[j][j] = 1.0/tjj**2 - S/tjj
+            for i in reversed(range(j)):
+                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
+        return res
+    
+    def inverse(self):
+        aux = self.Cholesky()
+        res = aux.CholeskyInverse()
+        return res
+    
+    def __repr__(self):
+        return repr(self.value)
+
+
+########################################
+
+# Implement the filter function below
+
+def filter(x, P):
+    for n in range(len(measurements)):
+        
+        # measurement update
+        z = measurements[n]
+        y = matrix([[z]]) - H * x
+        S = H * P * H.transpose() + R
+        K = P * H.transpose() * S.inverse()
+        
+        x = x + K * y
+        P = (I - K * H) * P
+        # prediction
+        x = F * x + u 
+        P = F * P * F.transpose()
+        
+        print 'x= '
+        x.show()
+        print 'P= '
+        P.show()
+
+
+
+########################################
+
+measurements = [1, 2, 3]
+
+x = matrix([[0.], [0.]]) # initial state (location and velocity)
+P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
+u = matrix([[0.], [0.]]) # external motion
+F = matrix([[1., 1.], [0, 1.]]) # next state function
+H = matrix([[1., 0.]]) # measurement function
+R = matrix([[1.]]) # measurement uncertainty
+I = matrix([[1., 0.], [0., 1.]]) # identity matrix
+
+filter(x, P)

unit2/u2-5_coconuts.py

+#Write a program that will find the initial number
+#of coconuts. 
+
+def f(n):
+    return (n-1) / 5.0 * 4
+
+def f6(n):
+    for i in range(6):
+        n = f(n)
+    return n 
+
+def is_int(n):
+    return abs(n-int(n)) < 0.0000001
+   
+# Enter code here.
+n = 1
+while not is_int(f6(n)):
+    n +=1
+
+print n

unit2/u2-hw6_fillmatrices.py

+# Fill in the matrices P, F, H, R and I at the bottom
+#
+# This question requires NO CODING, just fill in the 
+# matrices where indicated. Please do not delete or modify
+# any provided code OR comments. Good luck!
+
+from math import *
+
+class matrix:
+    
+    # implements basic operations of a matrix class
+    
+    def __init__(self, value):
+        self.value = value
+        self.dimx = len(value)
+        self.dimy = len(value[0])
+        if value == [[]]:
+            self.dimx = 0
+    
+    def zero(self, dimx, dimy):
+        # check if valid dimensions
+        if dimx < 1 or dimy < 1:
+            raise ValueError, "Invalid size of matrix"
+        else:
+            self.dimx = dimx
+            self.dimy = dimy
+            self.value = [[0 for row in range(dimy)] for col in range(dimx)]
+    
+    def identity(self, dim):
+        # check if valid dimension
+        if dim < 1:
+            raise ValueError, "Invalid size of matrix"
+        else:
+            self.dimx = dim
+            self.dimy = dim
+            self.value = [[0 for row in range(dim)] for col in range(dim)]
+            for i in range(dim):
+                self.value[i][i] = 1
+    
+    def show(self):
+        for i in range(self.dimx):
+            print self.value[i]
+        print ' '
+    
+    def __add__(self, other):
+        # check if correct dimensions
+        if self.dimx != other.dimx or self.dimy != other.dimy:
+            raise ValueError, "Matrices must be of equal dimensions to add"
+        else:
+            # add if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, self.dimy)
+            for i in range(self.dimx):
+                for j in range(self.dimy):
+                    res.value[i][j] = self.value[i][j] + other.value[i][j]
+            return res
+    
+    def __sub__(self, other):
+        # check if correct dimensions
+        if self.dimx != other.dimx or self.dimy != other.dimy:
+            raise ValueError, "Matrices must be of equal dimensions to subtract"
+        else:
+            # subtract if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, self.dimy)
+            for i in range(self.dimx):
+                for j in range(self.dimy):
+                    res.value[i][j] = self.value[i][j] - other.value[i][j]
+            return res
+    
+    def __mul__(self, other):
+        # check if correct dimensions
+        if self.dimy != other.dimx:
+            raise ValueError, "Matrices must be m*n and n*p to multiply"
+        else:
+            # subtract if correct dimensions
+            res = matrix([[]])
+            res.zero(self.dimx, other.dimy)
+            for i in range(self.dimx):
+                for j in range(other.dimy):
+                    for k in range(self.dimy):
+                        res.value[i][j] += self.value[i][k] * other.value[k][j]
+            return res
+    
+    def transpose(self):
+        # compute transpose
+        res = matrix([[]])
+        res.zero(self.dimy, self.dimx)
+        for i in range(self.dimx):
+            for j in range(self.dimy):
+                res.value[j][i] = self.value[i][j]
+        return res
+    
+    # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
+    
+    def Cholesky(self, ztol=1.0e-5):
+        # Computes the upper triangular Cholesky factorization of
+        # a positive definite matrix.
+        res = matrix([[]])
+        res.zero(self.dimx, self.dimx)
+        
+        for i in range(self.dimx):
+            S = sum([(res.value[k][i])**2 for k in range(i)])
+            d = self.value[i][i] - S
+            if abs(d) < ztol:
+                res.value[i][i] = 0.0
+            else:
+                if d < 0.0:
+                    raise ValueError, "Matrix not positive-definite"
+                res.value[i][i] = sqrt(d)
+            for j in range(i+1, self.dimx):
+                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
+                if abs(S) < ztol:
+                    S = 0.0
+                res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
+        return res
+    
+    def CholeskyInverse(self):
+        # Computes inverse of matrix given its Cholesky upper Triangular
+        # decomposition of matrix.
+        res = matrix([[]])
+        res.zero(self.dimx, self.dimx)
+        
+        # Backward step for inverse.
+        for j in reversed(range(self.dimx)):
+            tjj = self.value[j][j]
+            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
+            res.value[j][j] = 1.0/tjj**2 - S/tjj
+            for i in reversed(range(j)):
+                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
+        return res
+    
+    def inverse(self):
+        aux = self.Cholesky()
+        res = aux.CholeskyInverse()
+        return res
+    
+    def __repr__(self):
+        return repr(self.value)
+
+
+########################################
+
+def filter(x, P):
+    for n in range(len(measurements)):
+        
+        # prediction
+        x = (F * x) + u
+        P = F * P * F.transpose()
+        
+        # measurement update
+        Z = matrix([measurements[n]])
+        y = Z.transpose() - (H * x)
+        S = H * P * H.transpose() + R
+        K = P * H.transpose() * S.inverse()
+        x = x + (K * y)
+        P = (I - (K * H)) * P
+    
+    print 'x= '
+    x.show()
+    print 'P= '
+    P.show()
+
+########################################
+
+print "### 4-dimensional example ###"
+
+measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
+initial_xy = [4., 12.]
+
+# measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
+# initial_xy = [-4., 8.]
+
+# measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]