Fixing issues with matrices in matlab functions, converting to cpp