Add Matlab code
Cleanup