Tests upgrade

MATLAB:
- some refactoring (functions)
- added tests