My Matlab Function
My 2012 paper implementaption