Added original matlab code