Fixed the Particle Filter, added collision handling logic and velocity change support code.