Adds matlab impl of the algorithm for hw