Contents

## 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 Solution

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 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.

## 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

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.

## Final Solution

To implement the full particle filter, the only thing that's really missing is the measurement_prob function. That's a little bit more involved, because I have to really compare what the exact measurement would be for any robot 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's comes a little flag that I defined. If I set it to zero then the sense function acts noise-free, which is what I want to complete the measurement model. But even if we left this out, you're going to get a fine answer, in my opinion. That makes it a little bit more accurate. That allows me to compute the exact bearings of the landmarks for my particle. Then I can compare these correct bearings called "predicted_measurements" with the ones I received. I do this down here in the compute errors routine where I go through each measurement, and in 2 steps I calculate the error in bearing. First, it's the absolute difference between my measurement that I observed minus the predicted measurement. There's an [ i ] at the end over here--let's see if you can see this. Right there.