Matlab algorithm