Continued work on MATLAB implementation of astar