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.