First set of MATLAB code that was tested