# Commits

committed 164c0a0 Draft

initial commit, added codes

• Participants

# Files changed (66)

`+=======================================`
`+Udacity 373 - programming autonmous car`
`+=======================================`
`+`
`+Python solutions for homeworks and exams.`
`+`
`+`

# hw3_6.py

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

# hw3_simulator.py

`+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()`

# hw4_5.py

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

# hw5_3.py

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

# hw5_4.py

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