English subtitles

← 01ps-08 Localization Program Solution

dummy description

Get Embed Code
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.
  33. Let me start with sense.
  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.