Changed matlab implementation to leverage c++ code