# alexband/Udacity-373

1 parent dc1c395 commit cca3c6a178a0e0ecef26028bb8378f3f7efc3171 huanghuang committed Mar 11, 2012
Showing with 279 additions and 0 deletions.
1. unit3.py → 3-10.py
2. +127 −0 3-13.py
3. +152 −0 3-19.py
4. cd373-unit3-notes.pdf
0 unit3.py → 3-10.py
File renamed without changes.
127 3-13.py
 @@ -0,0 +1,127 @@ + +# Now we want to simulate robot +# motion with our particles. +# Each particle should turn by 0.1 +# and then move by 5. +# +# +# Don't modify the code below. Please enter +# your code at the bottom. + +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 #### + +N = 1000 +p = [] +for i in range(N): + x = robot() + p.append(x) +p = map(lambda x: x.move(0.1, 5), p) +print p #PLEASE LEAVE THIS HERE FOR GRADING PURPOSES
152 3-19.py
 @@ -0,0 +1,152 @@ +# 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 = [] + +#weighted sample solution 1 +import bisect +for i in xrange(N): + acc = [] + s = 0 + for weight in w: + s += weight + acc.append(s) + p3.append(p[bisect.bisect(acc, random.random()*s)]) + +#weighted sample solution 2 + + +
BIN cd373-unit3-notes.pdf
Binary file not shown.