Built augmented state matrices, added cost function weights and control constraints. Also, added formQPMatrices.m and tested it for a horizon of 10.

Outer loop of code set up. Just need the inner loop where we set up the QP problem (formQPMatrices) and call the solver.