Amcl Improvements

Amcl update:

Main update is the calculation of the particle mean when the resampling is not done (but motion and observation is integrated) - so that amcl publishes corrections every time an observation is integrated. 

Also added drawing of the corrected laser points (for use when comparing with ground truth). 
I would just get rid of this?