Added MATLAB code and commented c++ code