Debugged longitudinal tracker (to be continued...)