Localización Montecarlo

Estoy implementando la localización de Montecarlo para mi robot que recibe un mapa del entorno y su ubicación inicial y orientación. El enfoque de la mina es el siguiente:

1. Crea uniformemente 500 partículas alrededor de la posición dada
2. Luego, en cada paso:
   • Actualización de movimiento de todas las partículas con odometría (mi enfoque actual es newX = oldX + odometryX (1 + standardGaussianRandom), etc.)
   • asigne peso a cada partícula utilizando datos de sonda (la fórmula es para la probabilidad de cada sensor * = gaussianPDF (realReading) donde gaussian tiene la media de lectura predicha)
   • devolver la partícula con mayor probabilidad como la ubicación en este paso
   • luego se vuelven a muestrear 9/10 de nuevas partículas de las antiguas según los pesos y 1/10 se muestrea de manera uniforme alrededor de la posición predicha

Ahora, escribí un simulador para el entorno del robot y así es como se comporta esta localización: http://www.youtube.com/watch?v=q7q3cqktwZI

Tengo mucho miedo de que durante un período de tiempo más largo el robot pueda perderse. Si agrega partículas a un área más amplia, el robot se pierde aún más fácilmente.

Espero un mejor rendimiento. ¿Algún consejo?

Acabo de responder la pregunta en StackOverflow, donde ya se ha preguntado. Aqui esta el link:

/programming/21249131/monte-carlo-localization-for-mobile-robot