Working on Matlab implementation