Added instrumentation for path following: location of lookahead, and bearing to lookahead.
Added logging for lookahead / path following data