updated matlab code, still not identical to R version