I managed to speed the particle filter up a bit - by eliminating the service calls - but I'm still not satisfied by the quality of the localisation