More sensor updates (to be tested) and threadpooling to make the particle filter run faster.
Merge branch 'master' of https://github.com/rss-2014-team-6/rss-team-6