Matlab implementation