Vehicle refactor 2014

Review Commits below.

Running up against the sternberg issue when running forward visual servoing... simulator now works though.