PS6 - Problem 1 complete. Unpolished Matlab code