Hi there,
You have a linear system and quadratic objective, so best is to to start from an infinite-horizon unconstrained controller (lqr in Matlab).
Compare that result with an unconstrained MPC with long horizon; and only then check behavior when constraints are added and horizon becomes very short.
The jump from infinite-horizon to finite-horizon brings stability issues. It may be that you need a terminal/arrival cost as well as a terminal set constraint.
Consult any textbook on linear MPC for this.
Some remarks on your code:
- D has wrong dimensions
- your joint x,y update is wrong; write y=Cx+Du before x=Ax+Bu
- you have a single Opti instance that you grow by adding variables and constraints all the time.
Instead, do a fresh Opti instance for each sampling time, or do a parametric one upfront.
Best regards,
Joris