# public alexband /Udacity-373

### Subversion checkout URL

You can clone with HTTPS or Subversion.

final

authored

Showing 2 changed files with 233 additions and 23 deletions.

1. hw3-final.py
2. partical_gui.py
87  hw3-final.py
 `@@ -139,16 +139,57 @@ def __repr__(self): #allows us to print robot attributes.` 139 139 ` # according to the noise parameters` 140 140 ` # self.steering_noise` 141 141 ` # self.distance_noise` 142 `+ def move(self, motion): # Do not change the name of this function` 143 `+` 144 `+ # ADD CODE HERE` 145 `+ steer = motion[0] + random.gauss(0.0,self.steering_noise)` 146 `+ distance = motion[1] + random.gauss(0.0, self.distance_noise)` 147 `+ #if distance < 0:` 148 `+ # raise ValueError, 'Robot cant move backwards'` 149 `+ theta = self.orientation` 150 `+ beta = distance / self.length * tan(steer)` 151 `+ if abs(beta) >= 0.0001:` 152 `+ radius = distance / beta` 153 `+ Cx = self.x - radius * sin(theta)` 154 `+ Cy = self.y + radius * cos(theta)` 155 `+ x_prime = Cx + radius * sin(beta+theta)` 156 `+ y_prime = Cy - radius * cos(beta+theta)` 157 `+ theta_prime = (theta + beta) % (2 * pi)` 158 `+ result = robot()` 159 `+ result.set(x_prime, y_prime, theta_prime)` 160 `+ else:` 161 `+ x_prime = self.x + distance * cos(theta)` 162 `+ y_prime = self.y + distance * sin(theta)` 163 `+ theta_prime = (theta + beta) % (2 * pi)` 164 `+ result = robot()` 165 `+ result.set(x_prime, y_prime, theta_prime)` 166 `+` 167 `+ result.set_noise(self.bearing_noise, self.steering_noise,self.distance_noise)` 168 `+ return result # make sure your move function returns an instance` 142 169 ` ` 143 170 ` # --------` 144 171 ` # sense: ` 145 172 ` # ` 146 `-` 147 173 ` # copy your code from the previous exercise` 148 174 ` # and modify it so that it simulates bearing noise` 149 175 ` # according to` 150 176 ` # self.bearing_noise` 151 177 ` ` 178 `+ def sense(self, nonoise=None): #do not change the name of this function` 179 `+ Z = []` 180 `+ if nonoise and nonoise == 0:` 181 `+ orientation = self.orientation + random.gauss(0.0, self.bearing_noise)` 182 `+ else:` 183 `+ orientation = self.orientation` 184 `+` 185 `+ bearing1 = atan2(landmarks[0][0]-self.y, landmarks[0][1]-self.x) - orientation + 2*pi` 186 `+ bearing2 = atan2(landmarks[1][0]-self.y, landmarks[1][1]-self.x) - orientation + 2*pi` 187 `+ bearing3 = atan2(landmarks[2][0]-self.y, landmarks[2][1]-self.x) - orientation` 188 `+ bearing4 = atan2(landmarks[3][0]-self.y, landmarks[3][1]-self.x) - orientation` 189 `+ Z = [bearing1, bearing2, bearing3, bearing4]` 190 `+` 191 `+ return Z #Leave this line here. Return vector Z of 4 bearings.` 192 `+` 152 193 ` ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################` 153 194 ` ` 154 195 ` # --------` `@@ -295,34 +336,34 @@ def particle_filter(motions, measurements, N=500): # I know it's tempting, but d` 295 336 ` ## vector near [x=93.476 y=75.186 orient=5.2664], that is, the` 296 337 ` ## robot's true location.` 297 338 ` ##` 298 `-##motions = [[2. * pi / 10, 20.] for row in range(8)]` 299 `-##measurements = [[4.746936, 3.859782, 3.045217, 2.045506],` 300 `-## [3.510067, 2.916300, 2.146394, 1.598332],` 301 `-## [2.972469, 2.407489, 1.588474, 1.611094],` 302 `-## [1.906178, 1.193329, 0.619356, 0.807930],` 303 `-## [1.352825, 0.662233, 0.144927, 0.799090],` 304 `-## [0.856150, 0.214590, 5.651497, 1.062401],` 305 `-## [0.194460, 5.660382, 4.761072, 2.471682],` 306 `-## [5.717342, 4.736780, 3.909599, 2.342536]]` 307 `-##` 308 `-##print particle_filter(motions, measurements)` 339 `+motions = [[2. * pi / 10, 20.] for row in range(8)]` 340 `+measurements = [[4.746936, 3.859782, 3.045217, 2.045506],` 341 `+ [3.510067, 2.916300, 2.146394, 1.598332],` 342 `+ [2.972469, 2.407489, 1.588474, 1.611094],` 343 `+ [1.906178, 1.193329, 0.619356, 0.807930],` 344 `+ [1.352825, 0.662233, 0.144927, 0.799090],` 345 `+ [0.856150, 0.214590, 5.651497, 1.062401],` 346 `+ [0.194460, 5.660382, 4.761072, 2.471682],` 347 `+ [5.717342, 4.736780, 3.909599, 2.342536]]` 348 `+` 349 `+print particle_filter(motions, measurements)` 309 350 ` ` 310 351 ` ## 2) You can generate your own test cases by generating` 311 352 ` ## measurements using the generate_ground_truth function.` 312 353 ` ## It will print the robot's last location when calling it.` 313 354 ` ##` 314 355 ` ##` 315 `-##number_of_iterations = 6` 316 `-##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]` 317 `-##` 318 `-##x = generate_ground_truth(motions)` 319 `-##final_robot = x[0]` 320 `-##measurements = x[1]` 321 `-##estimated_position = particle_filter(motions, measurements)` 322 `-##print_measurements(measurements)` 323 `-##print 'Ground truth: ', final_robot` 324 `-##print 'Particle filter: ', estimated_position` 325 `-##print 'Code check: ', check_output(final_robot, estimated_position)` 356 `+number_of_iterations = 6` 357 `+motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]` 358 `+` 359 `+x = generate_ground_truth(motions)` 360 `+final_robot = x[0]` 361 `+measurements = x[1]` 362 `+estimated_position = particle_filter(motions, measurements)` 363 `+print_measurements(measurements)` 364 `+print 'Ground truth: ', final_robot` 365 `+print 'Particle filter: ', estimated_position` 366 `+print 'Code check: ', check_output(final_robot, estimated_position)` 326 367 ` ` 327 368 ` ` 328 369 ` `
169  partical_gui.py
 ... ... `@@ -0,0 +1,169 @@` 1 `+#Developped with Python 2.7.2` 2 `+#` 3 `+# How to use it :` 4 `+# 1) Copy and save this file` 5 `+# 2) Copy your code in homework 3-6 and` 6 `+#save it as hw3_6_ParticleFilter.py in the` 7 `+#same directory` 8 `+# 3) Execute this file` 9 `+` 10 `+from Tkinter import *` 11 `+from hw3_6_ParticleFilter import *` 12 `+` 13 `+#***************************************` 14 `+class DispParticleFilter(Tk):` 15 `+` 16 `+ def __init__(self, motions, measurements, N=500 ):` 17 `+ Tk.__init__(self)` 18 `+ self.title( 'Diplay Particle Filter CS373-HW03.06')` 19 `+ self.motions = motions` 20 `+ self.measurements = measurements` 21 `+ self.N = N` 22 `+ #init particle filter` 23 `+ self.initFilter()` 24 `+ # Drawing` 25 `+ self.margin = 100 # margin` 26 `+ self.zoom_factor = 2 # zoom factor` 27 `+ self.can = DisplayParticles ( self.margin, self.zoom_factor )` 28 `+ self.can.configure(bg ='ivory', bd =2, relief=SUNKEN)` 29 `+ self.can.pack(side =TOP, padx =5, pady =5)` 30 `+ self.can.draw_all(self.p)` 31 `+ #Buttons` 32 `+ self.button1 = Button(self, text ='Reset', command =self.resetFilter)` 33 `+ self.button1.pack(side =LEFT, padx =5, pady =5)` 34 `+ self.button2 = Button(self, text ='Next step', command =self.nextStep)` 35 `+ self.button2.pack(side =LEFT, padx =5, pady =5)` 36 `+ #Label` 37 `+ textLabel = 'Current state = ' + str(self.actualState) + '/' + str(len(motions)-1)` 38 `+ self.label = Label(self, text = textLabel )` 39 `+ self.label.pack(side =BOTTOM, padx =5, pady =5)` 40 `+` 41 `+ def resetFilter(self):` 42 `+ self.initFilter()` 43 `+ #Replot all` 44 `+ self.robot = get_position(self.p)` 45 `+ self.can.draw_all(self.p)` 46 `+` 47 `+ def initFilter (self):` 48 `+ # Make particles` 49 `+ self.p = [] # p : particles set` 50 `+ for i in range(self.N):` 51 `+ r = robot()` 52 `+ r.set_noise(bearing_noise, steering_noise, distance_noise)` 53 `+ self.p.append(r)` 54 `+ # --------------` 55 `+ self.actualState = 0` 56 `+` 57 `+ def nextStep (self, event=None):` 58 `+ self.actualState = self.actualState + 1` 59 `+ if self.actualState < len(self.motions):` 60 `+ #Label` 61 `+ stateString = 'Actual state = ' + str(self.actualState) + '/' + str(len(motions)-1)` 62 `+ self.label.configure( text = stateString )` 63 `+ # motion update (prediction)` 64 `+ p2 = []` 65 `+ for i in range(self.N):` 66 `+ p2.append(self.p[i].move(self.motions[self.actualState]))` 67 `+ self.p = p2` 68 `+ # measurement update` 69 `+ w = []` 70 `+ for i in range(self.N):` 71 `+ w.append(self.p[i].measurement_prob(self.measurements[self.actualState]))` 72 `+ # resampling` 73 `+ p3 = []` 74 `+ index = int(random.random() * self.N)` 75 `+ beta = 0.0` 76 `+ mw = max(w)` 77 `+ for i in range(self.N):` 78 `+ beta += random.random() * 2.0 * mw` 79 `+ while beta > w[index]:` 80 `+ beta -= w[index]` 81 `+ index = (index + 1) % self.N` 82 `+ p3.append(self.p[index])` 83 `+ self.p = p3` 84 `+ #Replot all` 85 `+ self.robot = get_position(self.p)` 86 `+ self.can.draw_all(self.p)` 87 `+` 88 `+class DisplayParticles(Canvas):` 89 `+` 90 `+ def __init__(self, margin, zoom_factor ):` 91 `+ Canvas.__init__(self)` 92 `+ #self.p = p` 93 `+ self.margin = margin` 94 `+ self.zoom_factor = zoom_factor` 95 `+ self.larg = (2*margin + world_size) * zoom_factor` 96 `+ self.haut = self.larg` 97 `+ self.configure(width=self.larg, height=self.haut )` 98 `+ self.larg, self.haut = (2*margin + world_size) * zoom_factor, (2*margin + world_size) * zoom_factor` 99 `+ # Landmarks` 100 `+ self.landmarks_radius = 2` 101 `+ self.landmarks_color = 'green'` 102 `+ # Particles` 103 `+ self.particle_radius = 1` 104 `+ self.particle_color = 'red'` 105 `+ # Robot` 106 `+ self.robot_radius = 4` 107 `+ self.robot_color = 'blue'` 108 `+` 109 `+ def draw_all(self, p):` 110 `+ #print len(p)` 111 `+ self.configure(bg ='ivory', bd =2, relief=SUNKEN)` 112 `+ self.delete(ALL)` 113 `+ self.p = p` 114 `+ self.plot_particles()` 115 `+ self.plot_landmarks( landmarks, self.landmarks_radius, self.landmarks_color )` 116 `+ self.robot = get_position(self.p)` 117 `+ self.plot_robot( self.robot_radius, self.robot_color)` 118 `+` 119 `+ def plot_landmarks(self, lms, radius, l_color ):` 120 `+ for lm in lms:` 121 `+ x0 = (self.margin + lm[1] - radius) * self.zoom_factor` 122 `+ y0 = (self.margin + lm[0] - radius) * self.zoom_factor` 123 `+ x1 = (self.margin + lm[1] + radius) * self.zoom_factor` 124 `+ y1 = (self.margin + lm[0] + radius) * self.zoom_factor` 125 `+ self.create_oval( x0, y0, x1, y1, fill = l_color )` 126 `+` 127 `+ def plot_particles(self):` 128 `+ for particle in self.p:` 129 `+ self.draw_particle( particle, self.particle_radius, self.particle_color )` 130 `+` 131 `+ def draw_particle(self, particle, radius, p_color):` 132 `+ #x0 = (self.margin + particle.x - radius) * self.zoom_factor` 133 `+ #y0 = (self.margin + particle.y - radius) * self.zoom_factor` 134 `+ #x1 = (self.margin + particle.x + radius) * self.zoom_factor` 135 `+ #y1 = (self.margin + particle.y + radius) * self.zoom_factor` 136 `+ #self.create_oval( x0, y0, x1, y1, fill = p_color )` 137 `+ x2 = (self.margin + particle.x) * self.zoom_factor` 138 `+ y2 = (self.margin + particle.y) * self.zoom_factor` 139 `+ x3 = (self.margin + particle.x + 2*radius*cos(particle.orientation)) * self.zoom_factor` 140 `+ y3 = (self.margin + particle.y + 2*radius*sin(particle.orientation)) * self.zoom_factor` 141 `+ self.create_line( x2, y2, x3, y3, fill = p_color, width =self.zoom_factor,` 142 `+ arrow=LAST, arrowshape=(2*self.zoom_factor,` 143 `+ 3*self.zoom_factor,` 144 `+ 1*self.zoom_factor) )` 145 `+` 146 `+ def plot_robot(self, radius, r_color):` 147 `+ x0 = (self.margin + self.robot[0] - radius) * self.zoom_factor` 148 `+ y0 = (self.margin + self.robot[1] - radius) * self.zoom_factor` 149 `+ x1 = (self.margin + self.robot[0] + radius) * self.zoom_factor` 150 `+ y1 = (self.margin + self.robot[1] + radius) * self.zoom_factor` 151 `+ self.create_oval( x0, y0, x1, y1, fill = r_color )` 152 `+ x2 = (self.margin + self.robot[0]) * self.zoom_factor` 153 `+ y2 = (self.margin + self.robot[1]) * self.zoom_factor` 154 `+ x3 = (self.margin + self.robot[0] + 2*radius*cos(self.robot[2])) * self.zoom_factor` 155 `+ y3 = (self.margin + self.robot[1] + 2*radius*sin(self.robot[2])) * self.zoom_factor` 156 `+ self.create_line( x2, y2, x3, y3, fill = r_color, width =self.zoom_factor, arrow=LAST )` 157 `+` 158 `+#**************************************************` 159 `+` 160 `+if __name__ == "__main__":` 161 `+ #motions and measurements ( here copy of the dataset in hw3-6 )` 162 `+ number_of_iterations = 20` 163 `+ motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]` 164 `+ x = generate_ground_truth(motions)` 165 `+ final_robot = x[0]` 166 `+ measurements = x[1]` 167 `+ #Display window` 168 `+ wind = DispParticleFilter ( motions, measurements, 500 )` 169 `+ wind.mainloop()`