Some fixes in StateFollowRoute and StateInvestigatePoint