An Integrated Localisation, Motion Planning and Obstacle Avoidance Algorithm in Belief Space