Fixes math under setPoint(). Use setPoint to update position of the robot with real world coordinates relative to /map frame