MATLAB code final version