Resampling

p1, w1 = 0.6
p2, w2 = 1.2
p3, w3 = 2.4
p4, w4 = 0.6
p5, w5 = 1.2
P(p1)

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

myrobot = robot()

p = []
for i in range(N):
	r = robot()
	r.set_noise(0.05, 0.05, 5.0)
	p.append(r)

for t in range(T):
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()

length = 20.
steering_noise = 0.0
distance_noise = 0.0

myrobot = robot(length)
myrobot.set(0.0, 0.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)

motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]

T = len(motions)
print 'Robot: '.myrobot