Matlab implementation added