Add Matlab implementation