Also changed up fontsize so details are more legible in the report

the very first input throwing theta out of the constraints. To fix this,
I added a large incremental cost penalty on deltaThetaDot to penalize
large sudden changes. This allows us to keep constraints much more
reliablyh

Re-linearizing every step doesn't fix overshoot problems.

Next step is to try a ramp because big steps are an issue.

Added slack varible. New function simLinFast() so that we can re-linearize dynamics more frequently.

Starting to consider state constraints on balance angle (theta). Added theta to the augmented state to help with this.

Next step is to add slack variables to minimize constraint violation.

Fixed a typo in the A matrix that prevented error from appearing in the cost. This basically made the controller do nothing.

Fixed a bug in the update of augmented state that was having us track theta instead of phi.

Need a way to implement state constraints (not just constraints on the deltas). This would be nice for preventing the balancebot from falling.

Lastly, we might need slack variables. I think the controller breaks for large desired steps because it flips out when near constraints.

Put everything together. System is stable. Adjusted control limit for better performance. Not following reference commands currently.

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.