# udacity373_code / unit3 / u3-20_resampling_wheel.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 141 142 143 144 145 146``` ```# In this exercise, you should implement the # resampler shown in the previous video. 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() #### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW #### 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)) p3 = [] index = random.randint(0, N-1) beta = 0.0 for i in range(0, N): beta += random.uniform(0.0, max(w)) if w[index] < beta: beta -= w[index] index = (index + 1) % N else: p3.append(p[index]) p = p3 print p #please leave this print statement here for grading! ```
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.