Homework Assignment 3¶
Empty Cell¶
Welcome to homework assignment number 3. This is all about particle filters. All ask you three questions. Then we have a fairly involved programming assignment. Here is my first question. Consider the following world with 4 states–A, B, C, and D. Suppose we wish to initialize our estimate with uniformly drawn particles. My question is what is the probability that cell A is empty? I’m asking this for different values of N– N equals 1, 4, and 10.
Empty Cell Solution¶
For N equals 1 it is 0.75 with 3/4 chance that particles will find themselves in B, D, or C, and there’s only a 1/4 chance that a particle is in A. For N equals 4, it’s 0.316, which is the same as 0.75^4. For each particle we have a 75% chance to be outside A, and 0.75^4 are the chances of 4 particles are outside A. As we move on, we get 0.75^10 with a 0.0563.
Motion Question¶
Question 2–consider the same world as before with 4 states. Now we’re facing a situation in which there are 5 particles in A, 3 in B, 3 in C, and 1 in D. We will now take a motion step. This is not about measurements, just motion. Our robot moves with a 50% probability horizontally. For example, if it was in D, it would move to C. It moves with a 50% chance vertically but never diagonally, and it never stays in the same cell. After 1 motion step, how many particles do we expect in cell A, B, C, and D? Answer the same for the asymptote. If we move infinitely often, what is the distribution that we expect this to converge to? I somewhat sloppily wrote “after infinite steps.” This is really what does it converge to in the end.
Motion Question Solution¶
The correct answer of the 1 step is 3, 3, 3, and 3. For example, for cell A we can only get particles from B or C. Those will come to A with a 50% chance each. So 6 times 0.5 is 3 for A. Each cell has exactly 6 particles in its total neighbors, so therefore, each of those cells gets a 3. Asymptotically, this won’t change. It’s again 3, 3, 3, and 3. I apologize it is a little bit misleading here that all these answers are the same number, but that’s what this example gives us.
Single Particle¶
In question 3 I’d like to quiz your intuition. Suppose we run a particle filter with N equals 1 particle. What will happen? It’ll work just fine, perhaps with a slightly larger error. It ignores robot measurements. It ignores robot motion. It likely fails. Or none of the above. You can check multiple boxes on the left.
Single Particle Solution¶
The correct answer are it ignores measurements and will likely fail. It ignores measurements, because the measurement sets the weighting factor in this resampling process, but with only 1 particle the same particle will be resampled no matter what. So the chances of the particle to be resampled is 1. Therefore, it’s independent of the actual measurement. The robot truly ignores the measurements. Because it ignores the measurements, it’ll likely fail, so works fine is incorrect. For ignoring robot motion–that’s not the case. Even a single particle can be sampled forward. So these are the two correct answers over here.
Circular Motion¶
Here’s our programming assignment. In class you already programmed a particle filter for a really simplistic robot that was able to measure ranges to landmarks and moved pretty much like a trashbin. Now I’d like replace it with a more interesting robot that’s more realistic. In particular, I’d like you to use a car. Here’s a car. A car tends to have fixed tires and two steerable in the front. Suppose the location of our car in a coordinate system is given by its x-coordinate and its y-coordinate– I’m picking the halfway point on the rear axle as the reference point– and by its heading dire theta. The state will be x, y, and the orientation theta. Then this car also has a steering angle that is called the alpha. The question is how is the state effected by driving a certain distance d with a fixed steering angle, assuming the initial state is x, y, and theta. It turns out to answer this, I also need to know the length of the vehicle, which I will just call L for length that is a constant throughout out consideration. This is usually called a bicycle model. Obviously, it suffices to look at one pair of tires because the other one– at least in approximation–runs pretty much parallel. If we look at the robot locally where we have a steering angle, alpha, robot length L, and we’re driving the rear tire forward by distance D, then the robot will turn to the left, and it’s turning angle, beta, is proportional to the distance that the rear tire moves forward divided by the length of the vehicle times the tangent of the steering angle. Let’s now compute the changes of x, y, and theta in the local coordinate system. Realize the turning radius R of this robot is simply the distance that we drive forward divided by beta. It’s relatively simple math, which I don’t want to explain in detail. This means that the robot is turning around a point over here at cx and cy. After the turn, the vehicle is located somewhere over here. In global coordinates, here is the way we describe this. Cx is the x coordinate of the robot x minus–now we go to the left– the sine of the robot orientation before motion times radius R. Similarly, cy is this expression over here–y plus cosine of the orientation times R. Then after motion, we can go back from cx to cy to a new state over here simply by adding in the turning radius beta. That is, our new x is cx plus sine of theta plus beta times radius. Notice how this parallels this guy over here, except for two changes. What we previously subtracted we’re now adding, and we’re adding beta to the argument of the sine. The same with y, and the orientation is just increased by beta–modulo 2π. This all works if the robot is actually turning. If the robot were to go straight, then R would become infinity by this division over here. For small betas–that’s smaller than 0.001–we can approximate this all as straight motion. Our new x is the old x times our driven distance pointed in the cosine of the heading direction. Similarly for y we go in sine of heading direction, and the heading direction stays the same. You could add beta, which is basically zero, to be slight more precise or you could just use theta. It doesn’t really matter. In this programming assignment, I’d like you to implement this piece of math over here in our particle filter. To make sure we increment it correctly, I will give you some example data that you can check. In our first part, I’ve prepared a lot of software for you, basically copying the old particle filter software over, and removing the motion and the measurement model for now. In this I just want you to practice the motion model. We assume a length of the road of 20. We initialize the road with this length parameter, and for this first iteration we assume no steering noise and no distance noise. I set the robot to (0.0, 0.0, 0.0) in the beginning, and then I cache away a number of motions, The way to read those is this robot is moving by 10 total with the steering angle of zero. Then it moves another 10 with a steering angle of pi divided by 6. Then it moves 20, again with a steering angle of zero. A pi over 6 is a left steering, so the robot should change its coordinates in the beginning just in the x direction, because it’s facing an x direction over here. Then turn left a little bit, go forward, and go straight again. Scrolling down a little bit, I also give you the code to run the robot. We’ve created the robot here. You print the initial coordinate. Then for each of the motions in this list over here, we apply the myrobot equals myrobot.move command, and we print out the successive command. If you get this right, these are the numbers I would like to see. Initially, the robot position is 0, 0, 0, 0. That’s just the one over here. It’s out first print command. It then moves forward in the x direction by 10. The orientation stays 0 and so does y, because there’s no steering. Now we steer. This affects x. It doesn’t quite move 10. It only moves 9.86. In the y direction it only moves 1.433, and its new orientation is 0.2886. Then we move straight again, and now the x coordinate becomes 0.3903, y coordinate becomes 7.12, orientation 0.28. Your code should output exactly the same values also over here. Just to give you a second test–this is a sequence of 10 motions where the robot moves 10 forward and always turns right but an angle of 0.2 in radians. We look at the outputs we get the following array. You can see that the orientation starting at zero, which is the same as 2π, decreases all the way to 5.26, and you can also see that the robot starts running in a circle whereas initially we add almost 10 to the x direction and almost nothing in the y direction. As you come down here, we subtract quite a bit in the y direction, because now the robot is going in a circle. You should look at these numbers over here and see if your code matches these exact numbers that my code outputs.
# -----------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called move()
#
# that takes self and a motion vector (this
# motion vector contains a steering* angle and a
# distance) as input and returns an instance of the class
# robot with the appropriate x, y, and orientation
# for the given motion.
#
# *steering is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your move function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases which you are free to use at the
# bottom. If you uncomment them for testing, make sure you
# re-comment them before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car.
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
# move along a section of a circular path according to motion
#
def move(self, motion): # Do not change the name of this function
alpha, d = motion
if abs(alpha) > max_steering_angle:
raise ValueError, "alpha greater than max_steering_angle"
if d < 0.0:
raise ValueError, "going backwards is not allowed."
beta = ((1.0 * d) / self.length) * tan(alpha)
if beta < 0.001:
x_p = self.x + (d * cos(self.orientation))
y_p = self.y + (d * sin(self.orientation))
else:
R = (1.0 * d) / beta
cx = self.x - (sin(self.orientation) * R)
cy = self.y + (cos(self.orientation) * R)
x_p = cx + (sin(self.orientation + beta) * R)
y_p = cy - (cos(self.orientation + beta) * R)
theta_p = (self.orientation + beta) % (2 * pi)
result = robot(self.length)
result.set_noise(self.bearing_noise, self.steering_noise, self.distance_noise)
result.set(x_p, y_p, theta_p)
return result
# make sure your move function returns an instance
# of the robot class with the correct coordinates.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## --------
## TEST CASE:
##
## 1) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=10.0 y=0.0 orient=0.0]
## Robot: [x=19.861 y=1.4333 orient=0.2886]
## Robot: [x=39.034 y=7.1270 orient=0.2886]
##
##
length = 20.
bearing_noise = 0.0
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
for t in range(T):
myrobot = myrobot.move(motions[t])
print 'Robot: ', myrobot
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## 2) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=9.9828 y=0.5063 orient=0.1013]
## Robot: [x=19.863 y=2.0201 orient=0.2027]
## Robot: [x=29.539 y=4.5259 orient=0.3040]
## Robot: [x=38.913 y=7.9979 orient=0.4054]
## Robot: [x=47.887 y=12.400 orient=0.5067]
## Robot: [x=56.369 y=17.688 orient=0.6081]
## Robot: [x=64.273 y=23.807 orient=0.7094]
## Robot: [x=71.517 y=30.695 orient=0.8108]
## Robot: [x=78.027 y=38.280 orient=0.9121]
## Robot: [x=83.736 y=46.485 orient=1.0135]
##
##
##length = 20.
##bearing_noise = 0.0
##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.2, 10.] for row in range(10)]
##
##T = len(motions)
##
##print 'Robot: ', myrobot
##for t in range(T):
## myrobot = myrobot.move(motions[t])
## print 'Robot: ', myrobot
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
Circular Motion Solution¶
# -----------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called move()
#
# that takes self and a motion vector (this
# motion vector contains a steering* angle and a
# distance) as input and returns an instance of the class
# robot with the appropriate x, y, and orientation
# for the given motion.
#
# *steering is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your move function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases which you are free to use at the
# bottom. If you uncomment them for testing, make sure you
# re-comment them before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car.
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
# move along a section of a circular path according to motion
#
def move(self, motion): # Do not change the name of this function
steering = motion[0]
distance = motion[1]
tolerance = 0.001
if abs(steering) > max_steering_angle:
raise ValueError, "Exceeding max_steering_angle"
if distance < 0.0:
raise ValueError, "Moving backwards is not valid."
# make a new copy
res = robot()
res.length = self.length
res.bearing_noise = self.bearing_noise
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
turn = tan(steering2) * distance2 / res.length
if (abs(turn) < tolerance):
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
# make sure your move function returns an instance
# of the robot class with the correct coordinates.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## --------
## TEST CASE:
##
## 1) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=10.0 y=0.0 orient=0.0]
## Robot: [x=19.861 y=1.4333 orient=0.2886]
## Robot: [x=39.034 y=7.1270 orient=0.2886]
##
##
length = 20.
bearing_noise = 0.0
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
for t in range(T):
myrobot = myrobot.move(motions[t])
print 'Robot: ', myrobot
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## 2) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=9.9828 y=0.5063 orient=0.1013]
## Robot: [x=19.863 y=2.0201 orient=0.2027]
## Robot: [x=29.539 y=4.5259 orient=0.3040]
## Robot: [x=38.913 y=7.9979 orient=0.4054]
## Robot: [x=47.887 y=12.400 orient=0.5067]
## Robot: [x=56.369 y=17.688 orient=0.6081]
## Robot: [x=64.273 y=23.807 orient=0.7094]
## Robot: [x=71.517 y=30.695 orient=0.8108]
## Robot: [x=78.027 y=38.280 orient=0.9121]
## Robot: [x=83.736 y=46.485 orient=1.0135]
##
##
##length = 20.
##bearing_noise = 0.0
##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.2, 10.] for row in range(10)]
##
##T = len(motions)
##
##print 'Robot: ', myrobot
##for t in range(T):
## myrobot = myrobot.move(motions[t])
## print 'Robot: ', myrobot
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
Here’s a function “move” as a class function of the class robot that implements where I get my motion vector, and the motion vector is defined to be steering first, then distance. I have a few error checks here to make sure the steering doesn’t exceed the max steering angle, and the same is true for distance. I want it to be non-negative. As I go down, I now implement the motion model. Let’s just look a little bit more. I make a new robot copy as in my sample code in class. I copy all the narrowing parameters–length, bearing noise, steering noise, and distance noise. Nothing surprising here. Here I’ll apply the noise, which you don’t need it for the first implementation, but later on as we go on, you need it to actually add noise. I just add Gaussian noise with the corresponding steering noise and distance noise parameters. If I set the mean of the Gaussian to be the steering command and the distance command then this adds noise. I could have equally written steering plus random.gauss, zero, comma, and then the noise parameter. As I go down further, here is my execution of motion. My turning angle, I called “turn.” This is the tangents of the noisy steering times the distance moved divided by the robot length. As in my explanation of this question, I’m going to branch and see if my turn is significant enough. It’s smaller than tolerance, and tolerance was set about to 0.001. Then I just model a straight motion. I get my new robot coordinates by the old robot coordinates, moving in the orientation of the robot–cosine for x and sine for y. I increase my orientation by turn, which is likely essentially zero. In case I go beyond 0 or 2π, I do the modal operation here just to make sure my angles are nicely between 0 and 2π. The more interesting case–as we go down in this program you can see that I now calculate the radius as the noise distance divided by turning. Then I find the center of the circle around which I’m turning, using the exact same math I just gave you. I now first change the orientation to be the new orientation by adding turn to the old orientation, modal, or 2π. Then I plug the new orientation into the sine and cosine argument, multiply by radius, add to the center of the circle to get my result. This routine over here gives me exactly what I wanted.
Sensing¶
Now I want you to implement a measurement model, using the function sense, that is more characteristic of what’s often in the literature on robotics. Say we have a robot and we have a landmark, then the robot can measure their bearing or angle to this landmark relative to its own local coordinate system. Whereas before we measured ranges or distances, now we measure bearings or angles. We assume in the world there are 4 landmarks– L1, L2, L3, and L4. All of those are distinguishable. The measurement vector is a set of 4 bearings that correspond to those four different landmarks. When you implement this, I recommend you use the function arctan2, which is the generalization of arctangent that takes as input delta y and then delta x. Arctan 2 would give you the orientation of a vector in global coordinates. We then have to adjust for the fact that it’s relative to the robot’s local coordinates, which is done by subtracting the orientation of the robot. This should give you the implementation of a bearing to a landmark. With this implementation I add a variable called “bearing_noise,” which you probably already used because it was already referenced before. I set it to 0 just so that we have no noise, and you can just your code. We initialize the robot coordinates as 30 and 20. Motions are now irrelevant. But as I go down, I now give the following two lines of code. I print the coordinates as before, and I print the measurements. The robot is at 30/20, and the bearings for these landmarks will be 6.00, 3.72, 1.92, and 0.85. My question for you is can you implement the software the measures those bearings. If I changed the initial orientation of the robot to be pi over 5, I now get my new robot coordinates over here, and my measurement vector outputs me very different values. That’s because this robot is now rotated and therefore all the bearings to the landmarks do change. It’s 5.3, 3.1, 1.3, and 0.22. Implement a measurement function that gives me exactly those values.
# --------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called sense()
# that takes self as input
# and returns a list, Z, of the four bearings* to the 4
# different landmarks. you will have to use the robot's
# x and y position, as well as its orientation, to
# compute this.
#
# *bearing is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your sense function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases provided at the bottom which you are
# free to use. If you uncomment any of these cases for testing
# make sure that you re-comment it before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) form.
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),
str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# sense:
# obtains bearings from positions
#
def sense(self): #do not change the name of this function
Z = []
for landmark in landmarks:
y, x = landmark
delta_y = y - self.y
delta_x = x - self.x
Z.append((atan2(delta_y, delta_x) - self.orientation) % (2.0 * pi))
# ENTER CODE HERE
# HINT: You will probably need to use the function atan2()
return Z #Leave this line here. Return vector Z of 4 bearings.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
## --------
## TEST CASES:
##
## 1) The following code should print the list [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721]
##
##
length = 20.
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = robot(length)
myrobot.set(30.0, 20.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
print 'Robot: ', myrobot
print 'Measurements: ', myrobot.sense()
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
##
## 2) The following code should print the list [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352]
##
##
##length = 20.
##bearing_noise = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(30.0, 20.0, pi / 5.0)
##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
##
##print 'Robot: ', myrobot
##print 'Measurements: ', myrobot.sense()
##
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
Sensing Solution¶
Here’s my implementation of sense, the measurement model, as a function in the class robot. I produce a vector called Z, which I return in the end, which has 4 bearings. Then I go through all my landmarks, and you already have the landmarks in your code– –there’s 4 of them–and I use the atan2 function, which is the mathematical function for computing the angle. It takes the y value as the first argument, and the x value as the second argument. This is the local angle of a landmark relative to the robot coordinate. Because the robot has a global heading direction, I need to subtract this to get my bearing value. If I were to add noise, which is a flag over here, then I just produce a random noise adding variable. This is something you shouldn’t have implemented, but you need later as you implement the noise. Of course, I make sure that the bearing is normalized between 0 and 2π. I append them to the list and return it.
Final Quiz¶
Now, in our final programming exercise, I want you to put everything together and build a particle filter. I’m supplying you with code that has as gaps pretty much the 2 functions you just programmed–move and sense, and some additional code that I copied from class– the particle filter code that you’re familiar with and also code that helps you test your routines so you can make sure they’re correct. The key new thing you have to do is you have to work on the noise. There is now bearing noise and steering noise and distance noise. The code that you wrote didn’t have any of those. I want you now to modify your procedures to accommodate this noise– steering noise, distance noise, and bearing noise– and all of it should be Gaussian. Let’s go all the way to the end. There are two test cases. The first test case, which are uncommented so we can run it. What this is is it creates a sequence of robot motions. At each of these time steps the robot turns a little bit and moves forward. It also has 8 measurements, which are the bearings to the those 4 different landmarks. If I go up a little bit in the code, then you’ll find that the ground true final position was 93, 75, and 5.2. When I run it, it runs the routine particle filter with those motions and those measurements as an input. It produces an estimate, which is 94, 71, and 5.2, which isn’t exactly the same as up here, but it’s close. This is a particle filter working. I’m supplying quite a few functions for you. You should look around. One is called particle filters. That’s exactly the same code we used in class and constructed together. I just copied it over, so if you look through this you’ll find, hopefully, no surprise here. I’m also supplying you the measurement probability function, which is part of implementation. Lets just go there. Here is the measurement probability function. There is something non-trivial here. I compute the predicted measurements, and then I compute a Gaussian that measures the distance between the measurements passed into the routine and the predicted measurements computer over here. That’s all happening down here. Here’s my Gaussian function with the exponential. Then I return my Gaussian error. There should be no surprise here. What’s important is a little modification to the sense function that we haven’t seen before. I can now give the sense function a parameter, and I give it the parameter 0. It switches off the noise model, so you will need the noise model for forward simulation of the robot, but you don’t need it for computing the probability of the measurement. It augments your sense function to have a flag that if it’s set to 0 it switches off the noise modeling and gets you the predicted best possible measurements. What you have to do is you have to find the part in the code that says “only add/modify code below here.” You have to copy over your move function and then work in, as it says in the instructions, the steering noise and the distance noise and it’s Gaussian–I hope you know how to do this. Then you also have to plug in the sense function, and you also have to plug in bearing noise and make sure there’s a flag that allows you to switch off the bearing noise. It should be an optional flag, so it has to have a default value of the bearing noise being on. Otherwise your code won’t run. Here is how we will test your code. If you go to test case number 2, then I wrote a few extra functions for you that allow you to test your particle filters on many, many instances just like the ones we were using for testing that are all randomly generated. Let me just go through that code line-by-line. Our test case will be 8 steps long. There is the same motions vector we had before of a slight turn on the circle. “Generate<u>ground</u>truth” gives us a sequence of measurements and a robot position that we can split as follows, using a robot simulation. Then you run your particle filter over here, and the function “check_output” down here compares the final robot position, the ground truth position, with your particle filter position, estimated position, from here and gives us a single flag whether this is all correct. Let me just do this. We generate a robot that finished with final location 20, -29, and this orientation over here. The particle filter came up with 22, -31, and 0.14, which is close to the original. My code check said “True.” Let me run it again. Different values–still true. Run it again. Different values–still true. Now, it could happen that the code check says “False.” I just ran it 20 times, and it said true for me every single time. But I’ve seen it say “False.” The reason is it’s a randomized algorithm. It’s a particle filter. It might actually not have a particle at the right place. So when we test your routine, we’re going to code our own code check, check_output. We have our own function for this, but we’re going to run it multiple times. If you get it wrong once it’s not a big deal. In summary, you will have comment out all the test cases again. All you have to do is supply the missing function. You can test the correctness yourself. You can basically grade yourself with this test case over here, but when you submit it, have those commented out, because we have our own test software. All we’re going to test is whether your particle filter gives us a good estimate when we choose randomly the initial position of the robots, measurements, motions, and so on.
# --------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called sense()
# that takes self as input
# and returns a list, Z, of the four bearings* to the 4
# different landmarks. you will have to use the robot's
# x and y position, as well as its orientation, to
# compute this.
#
# *bearing is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your sense function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases provided at the bottom which you are
# free to use. If you uncomment any of these cases for testing
# make sure that you re-comment it before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) form.
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),
str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# sense:
# obtains bearings from positions
#
def sense(self, add_noise=1):
Z = []
for landmark in landmarks:
y, x = landmark
delta_y = y - self.y
delta_x = x - self.x
bearing = (atan2(delta_y, delta_x) - self.orientation)
if add_noise:
bearing = random.gauss(bearing, self.bearing_noise)
Z.append(bearing % (2.0 * pi))
# ENTER CODE HERE
# HINT: You will probably need to use the function atan2()
return Z #Leave this line here. Return vector Z of 4 bearings.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
## --------
## TEST CASES:
##
## 1) The following code should print the list [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721]
##
##
length = 20.
bearing_noise = 0.0
steering_noise = 0.0
distance_noise = 0.0
myrobot = robot(length)
myrobot.set(30.0, 20.0, 0.0)
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
print 'Robot: ', myrobot
print 'Measurements: ', myrobot.sense()
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
##
## 2) The following code should print the list [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352]
##
##
##length = 20.
##bearing_noise = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(30.0, 20.0, pi / 5.0)
##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
##
##print 'Robot: ', myrobot
##print 'Measurements: ', myrobot.sense()
##
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## sense function with randomized initial robot coordinates.
# --------------
# USER INSTRUCTIONS
#
# Now you will put everything together.
#
# First make sure that your sense and move functions
# work as expected for the test cases provided at the
# bottom of the previous two programming assignments.
# Once you are satisfied, copy your sense and move
# definitions into the robot class on this page, BUT
# now include noise.
#
# A good way to include noise in the sense step is to
# add Gaussian noise, centered at zero with variance
# of self.bearing_noise to each bearing. You can do this
# with the command random.gauss(0, self.bearing_noise)
#
# In the move step, you should make sure that your
# actual steering angle is chosen from a Gaussian
# distribution of steering angles. This distribution
# should be centered at the intended steering angle
# with variance of self.steering_noise.
#
# Feel free to use the included set_noise function.
#
# Please do not modify anything except where indicated
# below.
from math import *
import random
# --------
#
# some top level parameters
#
max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car.
bearing_noise = 0.1 # Noise parameter: should be included in sense function.
steering_noise = 0.1 # Noise parameter: should be included in move function.
distance_noise = 5.0 # Noise parameter: should be included in move function.
tolerance_xy = 15.0 # Tolerance for localization in the x and y directions.
tolerance_orientation = 0.25 # Tolerance for orientation.
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format.
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 20.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
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)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# measurement_prob
# computes the probability of a measurement
#
def measurement_prob(self, measurements):
# calculate the correct measurement
predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise.
# compute errors
error = 1.0
for i in range(len(measurements)):
error_bearing = abs(measurements[i] - predicted_measurements[i])
error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate
# update Gaussian
error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) /
sqrt(2.0 * pi * (self.bearing_noise ** 2)))
return error
def __repr__(self): #allows us to print robot attributes.
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y),
str(self.orientation))
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
#
def move(self, motion):
alpha, d = motion
if abs(alpha) > max_steering_angle:
raise ValueError, "alpha greater than max_steering_angle"
if d < 0.0:
raise ValueError, "going backwards is not allowed."
alpha = random.gauss(alpha, self.steering_noise)
d = random.gauss(d, self.distance_noise)
beta = ((1.0 * d) / self.length) * tan(alpha)
if beta < 0.001:
x_p = self.x + (d * cos(self.orientation))
y_p = self.y + (d * sin(self.orientation))
else:
R = (1.0 * d) / beta
cx = self.x - (sin(self.orientation) * R)
cy = self.y + (cos(self.orientation) * R)
x_p = cx + (sin(self.orientation + beta) * R)
y_p = cy - (cos(self.orientation + beta) * R)
theta_p = (self.orientation + beta) % (2 * pi)
result = robot(self.length)
result.set_noise(self.bearing_noise, self.steering_noise, self.distance_noise)
result.set(x_p, y_p, theta_p)
return result
# copy your code from the previous exercise
# and modify it so that it simulates motion noise
# according to the noise parameters
# self.steering_noise
# self.distance_noise
# --------
# sense:
def sense(self, add_noise=1):
Z = []
for landmark in landmarks:
y, x = landmark
delta_y = y - self.y
delta_x = x - self.x
bearing = (atan2(delta_y, delta_x) - self.orientation)
if add_noise:
bearing = random.gauss(bearing, self.bearing_noise)
Z.append(bearing % (2.0 * pi))
return Z
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
# --------
#
# extract position from a particle set
#
def get_position(p):
x = 0.0
y = 0.0
orientation = 0.0
for i in range(len(p)):
x += p[i].x
y += p[i].y
# orientation is tricky because it is cyclic. By normalizing
# around the first particle we are somewhat more robust to
# the 0=2pi problem
orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi))
+ p[0].orientation - pi)
return [x / len(p), y / len(p), orientation / len(p)]
# --------
#
# The following code generates the measurements vector
# You can use it to develop your solution.
#
def generate_ground_truth(motions):
myrobot = robot()
myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
Z = []
T = len(motions)
for t in range(T):
myrobot = myrobot.move(motions[t])
Z.append(myrobot.sense())
#print 'Robot: ', myrobot
return [myrobot, Z]
# --------
#
# The following code prints the measurements associated
# with generate_ground_truth
#
def print_measurements(Z):
T = len(Z)
print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \
(str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3]))
for t in range(1,T-1):
print ' [%.8s, %.8s, %.8s, %.8s],' % \
(str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3]))
print ' [%.8s, %.8s, %.8s, %.8s]]' % \
(str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3]))
# --------
#
# The following code checks to see if your particle filter
# localizes the robot to within the desired tolerances
# of the true position. The tolerances are defined at the top.
#
def check_output(final_robot, estimated_position):
error_x = abs(final_robot.x - estimated_position[0])
error_y = abs(final_robot.y - estimated_position[1])
error_orientation = abs(final_robot.orientation - estimated_position[2])
error_orientation = (error_orientation + pi) % (2.0 * pi) - pi
correct = error_x < tolerance_xy and error_y < tolerance_xy \
and error_orientation < tolerance_orientation
return correct
def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!
# --------
#
# Make particles
#
p = []
for i in range(N):
r = robot()
r.set_noise(bearing_noise, steering_noise, distance_noise)
p.append(r)
# --------
#
# Update particles
#
for t in range(len(motions)):
# motion update (prediction)
p2 = []
for i in range(N):
p2.append(p[i].move(motions[t]))
p = p2
# measurement update
w = []
for i in range(N):
w.append(p[i].measurement_prob(measurements[t]))
# resampling
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
return get_position(p)
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out.
##
## You can test whether your particle filter works using the
## function check_output (see test case 2). We will be using a similar
## function. Note: Even for a well-implemented particle filter this
## function occasionally returns False. This is because a particle
## filter is a randomized algorithm. We will be testing your code
## multiple times. Make sure check_output returns True at least 80%
## of the time.
## --------
## TEST CASES:
##
##1) Calling the particle_filter function with the following
## motions and measurements should return a [x,y,orientation]
## vector near [x=93.476 y=75.186 orient=5.2664], that is, the
## robot's true location.
##
motions = [[2. * pi / 10, 20.] for row in range(8)]
measurements = [[4.746936, 3.859782, 3.045217, 2.045506],
[3.510067, 2.916300, 2.146394, 1.598332],
[2.972469, 2.407489, 1.588474, 1.611094],
[1.906178, 1.193329, 0.619356, 0.807930],
[1.352825, 0.662233, 0.144927, 0.799090],
[0.856150, 0.214590, 5.651497, 1.062401],
[0.194460, 5.660382, 4.761072, 2.471682],
[5.717342, 4.736780, 3.909599, 2.342536]]
print particle_filter(motions, measurements)
## 2) You can generate your own test cases by generating
## measurements using the generate_ground_truth function.
## It will print the robot's last location when calling it.
##
##
##number_of_iterations = 6
##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]
##
##x = generate_ground_truth(motions)
##final_robot = x[0]
##measurements = x[1]
##estimated_position = particle_filter(motions, measurements)
##print_measurements(measurements)
##print 'Ground truth: ', final_robot
##print 'Particle filter: ', estimated_position
##print 'Code check: ', check_output(final_robot, estimated_position)
Final Quiz Solution¶
So to implement the full particle filter, the only thing is really missing is the measurement_prob function. And that’s a little bit more involved because I have to really compare what the exact measurement would be for any ove, overt particle. And what I sensed and compute the probability correspondence between the correct measurements, and what I sensed over here. To do this, I calculate predicted measurements using the sense function. Here comes a little flag that I defined. If I set it to 0, than the sense function acts noise free. Which is what I want, it could be the measurement model. But even we you left this out, you’re going to get a fine answer on my opinion. But that makes it a little bit more accurate. So that allows me to compute the exact bearings of the landmarks for my particle. And then I can compare these correct bearings called predicted measurements with the ones that I received. Now do this, down here, in the compute errors routine. Where I go through each measurement and in two steps, I calculated the error in bearing. First, it’s the absolute difference between my measurement that I observed, minus the predicted measurement, and there’s an i at the end over here. Let’s see if you can see this. Right there. And this difference might fall outside minus pi plus pi. So this line over here just brings it back to the smallest possible value in this cyclic space of 0 to 2 pi. So adding pi, adding more load 2 times pi and I subtract pi again. So this gives me a value between minus pi and plus pi. I then pluck this error_bearing into a Gaussian. And here is my Gaussian where I squared it, I divide it by my bearing-noise squared, complete the exponential, and use my normalizer to strictly speaking of, don’t really need for the implementation, I can safely omit it because weights are self-normalized. But I left it in, so it’s actually really a Gaussian. And I take this Gaussian value and multiply it up into my error function. So for each of the measurements, I multiply in one Gaussian. And the final Gaussian is my importance whether I return in this function over here. So this is not easy to implement. I hope you got it right. Scrolling further down in my code, I now implement the particle field as follows. It uses a thousand particles. And this is exactly the same routine we had before, where we generate our initial particles. Here, I set the noise for these particles, to be bearing_noise, steering_noise and distance_noise. I don’t comment out the measurement generation step, I just take the input. And then as I go further down, I just run my particle theta. This is the exact same code you’re familiar with. There’s a motion update, there’s a measurement update, and there’s a resampling step over here. All those are the same as before. And at the very end I just print the result of get_position. So if I do this for my example, here is the position I get. And I guess for, I forgot to uncommon the Robot coordinate over here. But if you look at the values over here, 7.0 is about the same as 8, 49 is about the same as 48, and 4.31 is about the same as 4.35. So this particle filter, clearly does a pretty job in estimating the forward position