Improved Matlab plots and added subfunctions