Implementation of some MATLAB algorithms in C++