I'm implementing Monte-Carlo localization for my robot that is given a map of the enviroment and its starting location and orientation. Mine approach is as follows:
- Uniformly create 500 particles around the given position
- Then at each step:
- motion update all the particles with odometry (my current approach is newX=oldX+ odometryX(1+standardGaussianRandom), etc.)
- assign weight to each particle using sonar data (formula is for each sensor probability*=gaussianPDF(realReading) where gaussian has the mean predictedReading)
- return the particle with biggest probability as the location at this step
- then 9/10 of new particles are resampled from the old ones according to weights and 1/10 is uniformly sampled around the predicted position
Now, I wrote a simulator for the robot's enviroment and here is how this localization behaves: http://www.youtube.com/watch?v=q7q3cqktwZI
I'm very afraid that for a longer period of time the robot may get lost. If add particles to a wider area, the robot gets lost even easier.
I expect a better performance. Any advice?