## ← Final Solution - Artificial Intelligence for Robotics

• 3 Followers
• 60 Lines

### Get Embed Code x Embed video Use the following code to embed this video. See our usage guide for more details on embedding. Paste this in your document somewhere (closest to the closing body tag is preferable): ```<script type="text/javascript" src='https://amara.org/embedder-iframe'></script> ``` Paste this inside your HTML body, where you want to include the widget: ```<div class="amara-embed" data-url="http://www.youtube.com/watch?v=zfEbYZ6hu-I" data-team="udacity"></div> ``` 2 Languages

• English [en]
• Japanese [ja]

Showing Revision 3 created 05/25/2016 by Udacity Robot.

1. So to implement the full particle filter,
2. the only thing is really missing is the measurement_prob function.
3. And that's a little bit more involved because I have to
4. really compare what the exact measurement would be for any ove, overt particle.
5. And what I sensed and compute the probability correspondence between
6. the correct measurements, and what I sensed over here.
7. To do this, I calculate predicted measurements using the sense function.
8. Here comes a little flag that I defined.
9. If I set it to 0, than the sense function acts noise free.
10. Which is what I want, it could be the measurement model.
11. But even we you left this out, you're going to get a fine answer on my opinion.
12. But that makes it a little bit more accurate.
13. So that allows me to compute the exact bearings of the landmarks for
14. my particle.
15. And then I can compare these correct bearings called predicted measurements
16. with the ones that I received.
17. Now do this, down here, in the compute errors routine.
18. Where I go through each measurement and in two steps,
19. I calculated the error in bearing.
20. First, it's the absolute difference between my measurement that I observed,
21. minus the predicted measurement, and there's an i at the end over here.
22. Let's see if you can see this.
23. Right there.
24. And this difference might fall outside minus pi plus pi.
25. So this line over here just brings it back
26. to the smallest possible value in this cyclic space of 0 to 2 pi.
28. So this gives me a value between minus pi and plus pi.
29. I then pluck this error_bearing into a Gaussian.
30. And here is my Gaussian where I squared it,
31. I divide it by my bearing-noise squared, complete the exponential, and
32. use my normalizer to strictly speaking of, don't really need for
33. the implementation, I can safely omit it because weights are self-normalized.
34. But I left it in, so it's actually really a Gaussian.
35. And I take this Gaussian value and multiply it up into my error function.
36. So for each of the measurements, I multiply in one Gaussian.
37. And the final Gaussian is my importance whether I return in this
38. function over here.
39. So this is not easy to implement.
40. I hope you got it right.
41. Scrolling further down in my code,
42. I now implement the particle field as follows.
43. It uses a thousand particles.
44. And this is exactly the same routine we had before,
45. where we generate our initial particles.
46. Here, I set the noise for
47. these particles, to be bearing_noise, steering_noise and distance_noise.
48. I don't comment out the measurement generation step, I just take the input.
49. And then as I go further down, I just run my particle theta.
50. This is the exact same code you're familiar with.
51. There's a motion update,
52. there's a measurement update, and there's a resampling step over here.
53. All those are the same as before.
54. And at the very end I just print the result of get_position.
55. So if I do this for my example, here is the position I get.
56. And I guess for, I forgot to uncommon the Robot coordinate over here.
57. But if you look at the values over here, 7.0 is about the same as 8,
58. 49 is about the same as 48, and 4.31 is about the same as 4.35.
59. So this particle filter,
60. clearly does a pretty job in estimating the forward position