## ← 01ps-08 Localization Program Solution

• 1 Follower
• 83 Lines

dummy description

### 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=37McU8k7FN8" data-team="udacity"></div> ``` 3 Languages

Showing Revision 1 created 10/24/2012 by Amara Bot.

1. Here is my solution to the programming assignment,
2. and it's quite straightforward given the class, but I'm really proud if you go it correct,
3. because it enabled you to program your own localization algorithm
4. very similar to the way we do it at the Google self-driving car.
5. First, I did two simple bookkeeping assignments.
6. I assigned a value to sensor_wrong as 1.0 minus the probability of sensor_right
7. and a probability of staying--that is, a motion failure--
8. as 1.0 minus the probability of p_move.
9. Let me scroll down very slowly.
10. Let me first go to my main routine.
11. I actually put a little check where the length of the measurements vector
12. is the same as the motions vector, and it would give me an error message if not.
13. Of course, this wasn't necessary for you.
14. I just did it because I want my software to look nice.
15. Then here is my initialization of my probability table.
16. I compute my initial uniform distribution by calculating the size of the array--
17. the number of rows times the number columns--
18. and then dividing 1.0 over the product of those
19. to be my initial distribution value.
20. This thing over here just builds up an array of the size of my colors array
21. but initializes it with the value of "pinit."
22. These two lines over here give me an initial uniform distribution, and then I iterate.
23. I go through the number of measurements, which is the same as number of motions.
24. I move first using the "move" command of which I provide my current distribution
25. and my motions command to obtain a new distribution.
26. Then I do the same with the sensing command.
27. I take my current distribution, the world itself, and the measurement vector
28. to obtain a new probability distribution.
29. When I've done this as many time as I have measurements and motions,
30. I output the final distribution.
31. So much for the main routine.
32. I now have to specify what move is and what sense is.
34. This is my sense routine. It goes from here to down here.
35. As an input, I have a probability distribution and my world map--
36. they're both of the same size--and a specific measurement, which is either red or green.
37. I construct and cite my new posterior distribution.
38. I initialize this with zeros, and I set the same size as my vector p.
39. In the inner loop, I now iterate over all elements in my grid cell.
40. I compute whether the measurement matches the color in the cell,
41. in which case I call it a hit.
42. Now my non-normalized posterior is the prior times this big sum over here.
43. It uses sensor_right if the measurement was correct
44. and sensor_wrong if the measurement was incorrect.
45. Finally, I add up all the values in aux--I do this with the variable "s."
46. Down here, I can normalize aux to have a total probability of 1, and then I return it.
47. The "move" command takes as an input a distribution and a motion vector.
48. It constructs the next distribution just like before as aux variable and sets it to zero.
49. Now I go through each grid cell and for each cell, I collect possible cells
50. that the robot might have come from.
51. With probability p_move, it actually moved,
52. in which case its prior coordinate would've been i minus the motion command.
53. That's because you go backwards in time.
54. This is a truncation, indicating we have a cyclic array,
55. and we do the same with j--let me scroll very carefully.
56. It's j minus the motion command, again in a cyclic fashion.
57. But it might've been we didn't move, in which case you just use the probability
58. of that specific cell multiplied by the probability of staying.
59. Now, this line together gives me the correct probability for the variable i and j.
60. I don't have to normalize, because it's not Bayes Rule.
61. I just return the corresponding posterior distribution.
62. I also have a little routine called "show" that goes through the entire probability field
63. and computes out all these probability vectors of p
64. that makes it slightly better formatted than just printing p in a single command.
65. If I run my software with a specific word over here,
66. it initializes p as 1/20 because there are 20 grid cells,
67. then runs 5 times through the motion command and the measurement command,
68. updates those, and then shows me the final result,
69. which I already explained, which is the array over here.
70. If you got this correct, then you've done something quite amazing.
71. You've programmed the core of Google's self-driving car localization methods.
72. In Google's case, the world isn't as simple as just red and green.
73. In Google's case, these are carefully assembled 2D surface models of the road surface.
74. But that doesn't affect what we've programed here.
75. It makes the measurement function slightly more involved.
76. The fact is that the thing we programmed here captures the key
77. of the probabilistic inference necessary to localize the Google care.
78. If you programmed this, you just have to replace the simple matching
79. of a measurement of green with an image matching of an entire imagery record
80. with imagery map.
81. I leave this as an exercise, because I can't do this in this Python environment here.
82. But I congratulate you that you really managed to do something quite amazing,
83. which is build an amazing piece of localization software.