Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

3-22

  • Loading branch information...
commit 001dde069d8ada09bed77d65e2fc9feea4c28e3f 1 parent bc0f9df
huanghuang authored
Showing with 152 additions and 0 deletions.
  1. +152 −0 3-22.py
View
152 3-22.py
@@ -0,0 +1,152 @@
+# In this exercise, write a program that will
+# run your previous code twice.
+# Please only modify the indicated area below!
+
+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/MODIFY CODE BELOW ####
+N = 1000
+T = 10
+
+myrobot = robot()
+myrobot = myrobot.move(0.1, 5.0)
+Z = myrobot.sense()
+
+p = []
+for i in range(N):
+ x = robot()
+ x.set_noise(0.05, 0.05, 5.0)
+ p.append(x)
+
+for t in range(T):
+ 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 = 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
+
+
+print p #Leave this print statement for grading purposes!
+
+
+
Please sign in to comment.
Something went wrong with that request. Please try again.