A-star path finding

This is an example for MATLAB. We could perhaps adapt it for our
problem.