Implementing robot control based on the particle filter

Still has some TODOs