implemented parts of algorithm in MatLab