Added some convenience stuff and fixed some Kalman filter on BeachBot issues
- locate node now waits if no /scan topic is published
- if the robot moves during pole initialization it restarts the process
- converted Kalman filter stuff from simulated data to real BeachBot data