Revised navigation code. Added cross track compensation.