The problem of localising a robot in a known environment is quite interesting. I would like to share with you my solution to the homework assignment for the unit 1 of the “CS 373 Programming a Robotic Car”.
The environment is a discrete map (colours matrix) where each area can be green or red. The robot can sense the colour (sense function) of the area where is currently on and can move in any directions (move function). To make things more interesting and close to reality the measurements and the motions are subjected to errors and this is represented by the variables sensor_right and p_move. Given a list of measurements and a list of motions the problem is to create a matrix of probability that describe where the robot is likely to be after all the specified moves.
More information about the definition of the problem can be found on Udacity.
I used the python module unittest for this purpose. This is one of the unit tests that I defined for the problem:
This is the main code that does the actual calculation:
The current algorithm used to solve this problem is called Montecarlo Localization.
You can find the full code in my personal repository.