Revert "Restructure MPC controller for LTV systems."

This reverts commit a73026bbb33ccac95cac6a0e90c4980d47804f28.
No longer have an LTV system because disturbances can handle the
time-dependence.
Worked on revised model and controller.