Changed optimal path following and fixed a bug on alpha. READY FOR LAKE TEST