Crosstrack error implementation.

Added crosstrack error calculation to current route.