Debug mode added, initial testing done

Added pieces for debugging with location information. First phase
testing done. Kalman filter error covariance needs to be tweaked to make
the kalman filter behave better.