Converted Matlab Code to C++ to Improve Performance