Autonomous intelligent vehicles : theory, algorithms, and implementation (2011)