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). 
How about this, is this still needed?