# udacity373_code / unit3 / u3-19_new_particles.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140``` ```# In this exercise, try to write a program that # will resample particles according to their weights. # Particles with higher weights should be sampled # more frequently (in proportion to their weight). # Don't modify anything below. Please scroll to the # bottom to enter your code. from math import * import random landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]] world_size = 100.0 class robot: def __init__(self): self.x = random.random() * world_size self.y = random.random() * world_size self.orientation = random.random() * 2.0 * pi self.forward_noise = 0.0; self.turn_noise = 0.0; self.sense_noise = 0.0; def set(self, new_x, new_y, new_orientation): if new_x < 0 or new_x >= world_size: raise ValueError, 'X coordinate out of bound' if new_y < 0 or new_y >= world_size: raise ValueError, 'Y coordinate out of bound' 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) def set_noise(self, new_f_noise, new_t_noise, new_s_noise): # makes it possible to change the noise parameters # this is often useful in particle filters self.forward_noise = float(new_f_noise); self.turn_noise = float(new_t_noise); self.sense_noise = float(new_s_noise); def sense(self): Z = [] for i in range(len(landmarks)): dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) dist += random.gauss(0.0, self.sense_noise) Z.append(dist) return Z def move(self, turn, forward): if forward < 0: raise ValueError, 'Robot cant move backwards' # turn, and add randomness to the turning command orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise) orientation %= 2 * pi # move, and add randomness to the motion command dist = float(forward) + random.gauss(0.0, self.forward_noise) x = self.x + (cos(orientation) * dist) y = self.y + (sin(orientation) * dist) x %= world_size # cyclic truncate y %= world_size # set particle res = robot() res.set(x, y, orientation) res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise) return res def Gaussian(self, mu, sigma, x): # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2)) def measurement_prob(self, measurement): # calculates how likely a measurement should be prob = 1.0; for i in range(len(landmarks)): dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2) prob *= self.Gaussian(dist, self.sense_noise, measurement[i]) return prob def __repr__(self): return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation)) def eval(r, p): sum = 0.0; for i in range(len(p)): # calculate mean error dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0) dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0) err = sqrt(dx * dx + dy * dy) sum += err return sum / float(len(p)) #myrobot = robot() #myrobot.set_noise(5.0, 0.1, 5.0) #myrobot.set(30.0, 50.0, pi/2) #myrobot = myrobot.move(-pi/2, 15.0) #print myrobot.sense() #myrobot = myrobot.move(-pi/2, 10.0) #print myrobot.sense() myrobot = robot() myrobot = myrobot.move(0.1, 5.0) Z = myrobot.sense() N = 1000 p = [] for i in range(N): x = robot() x.set_noise(0.05, 0.05, 5.0) p.append(x) p2 = [] for i in range(N): p2.append(p[i].move(0.1, 5.0)) p = p2 w = [] for i in range(N): w.append(p[i].measurement_prob(Z)) #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW #### # You should make sure that p3 contains a list with particles # resampled according to their weights. # Also, DO NOT MODIFY p. p3 = [] ```