Fixed bug in Matlab code