Fixed a major bug that was causing the map optimisation to sometimes...

Fixed a major bug that was causing the map optimisation to sometimes oscillate, causing residuals to grow. Fixed a bug with map warping using last observed to use scan correction not overall error. Fixed a few bugs with map warping by every scan