Matlab cleaned up

Arranged Matlab files, added path includes where necessary.

Added back in the standard ICP implementation so it can be used for
comparison

Implemented point to plane metric with outlier rejection.