Pull request for Matlab code

Thomas is debugging.