Added some new methods of evaluating MDP policies
Added robot velocity/position to simulated annealing approach.
Still need to add a heading filter in the retargeting function