Added MATLAB function, not reviewed yet