# Is there a better way to simulate quadratic cost MPC problems?

## Question

I would like to know if there is a more straightforward or a better way to simulate MPC problems in terms of code length and accuracy as compared to what I am doing. I am looking to know if there are some ready-made packages like we have YALMIP for MATLAB, however, any other alternative methods for simulating MPC problems that you have are welcome!

## What I have done

The receding horizon optimal control problem I want to simulate is as follows: \begin{aligned} \min_{\{u_0, ...,u_{N-1}\}} \; & \sum_{p=0}^{N-1} x_p^TPx_p + x_N^TQx_N \\ \text{subject to}\ \;& x_{k+1}=Ax_k+Bu_k,\; \; k = 0, 1,...,N-1 \\& A_x x_k \leq b_x, \;\; x_k \in \mathbb{R}^n, \;\; k = 0, 1...,N-1 \\ & A_u u_k \leq b_u, \;\; u_k \in \mathbb{R}^m, \;\; k = 0, 1...,N-1 \\ & A_N x_N \leq b_N, \;\; x_N \in \mathbb{R}^n\\ & x_0=x(0) \end{aligned} What I have done is translate the above problem into a form which I can give to Mathematica's QuadraticOptimization solver. Going as per Sec. 11.3.1 of Predictive Control for linear and hybrid systems (this book is available for free on author's website and this is not an illegitimate copy), the problem can be re-cast as below: \begin{aligned} \min_{V_0} \; & V_0^TMV_0 \\ \text{subject to}\ \;& J_0V_0 \leq w_0 \\& \begin{bmatrix} 0 & ... & 0 & 1 & ... & 1 \end{bmatrix}V_0=x(0) \end{aligned} where $$V_0=\{u_0,...,u_{N-1},x(0)\}$$ and the row matrix in second constraint has $$mN$$ zeros and $$n$$ ones, which basically means that the last $$n$$ components of $$V_0$$ make up the vector $$x(0)$$.

## A bit of a background

The above method works but I don't know if it gives correct results. I tried comparing my MPC results with a few examples from research papers and lecture notes and the outcome was a mixed bag. For some, results matched perfectly, for some results matched for certain time periods only and for some it threw the error of no points satisfies the constraints. It is of course an issue for a separate question on MathematicaSE but this is what prompted me to ask this question.

## Toy example

The translation of the problem from first formulation to second one is a lengthy task in coding and my code for that is too long and uses many custom packages. So, I have assigned the variables in second formulation some values for this example.

In[9]:=
(*control horizon, N=2*)
(*u_k, x_k, x_N  belong to Reals*)
x0 = {1};
J0 = {{1, 2, 3}, {2, 3, 4}};
w0 = {1, 2};
M = {{5, 6, 8}, {2, 5, 4}, {4, 6, 1}};
aeq = ArrayFlatten[{{ConstantArray[0, {1, 2}], IdentityMatrix[1]}}];
beq = -x0;
c = ConstantArray[0, 2 + 1];
V0 = QuadraticOptimization[{2 M, c}, {-J0, w0}, {aeq, beq}]

Out[16]= {-0.888889, -0.555556, 1.}

• You'll want to include your current code and an example as well. As it stands, I am not sure if the question is about the Mathematica implementation (which would be on topic), or the algorithm (which would probably be best asked elsewhere). Commented Mar 3, 2021 at 17:52
• @MarcoB My question is on Mathematica implementation: I have done this but are there ready-made packages or functions to do this or is there a different way to simulate such problems? Is an example still needed, given this is what I am asking? My actual code is messy with lots of functions defined in many custom packages, hence, my hesitation. A toy example could be created where I assign some values to each variable in the second formulation but I am not sure how helpful that would be. Commented Mar 3, 2021 at 18:02
• @MarcoB I have added a toy example. Is it fine or does it need changes? Commented Mar 3, 2021 at 18:26

(I find it hard to follow the notation you used in the toy example.)

But here's a simple system.

a = {{3}};
b = {{1}};
q = {{1}};
r = {{10}};
hor = 3;
cons = {-1 <= x <= 1, -1 <= u <= 1};


Assemble the cost function.

cost = Sum[{x[i]}.q.{x[i]} + {u[i]}.r.{u[i]}, {i, 0, hor - 1}] + {x[hor]}.q.{x[hor]};
stateEqCons = Table[Thread[{x[i]} -> a.{x[i - 1]} + b.{u[i - 1]}], {i, hor, 1, -1}];
cost1 = Fold[ReplaceAll[#1, #2] &, cost, stateEqCons]


And the constraints.

cons1 = Join[{cons[[1]] /. x -> x[0]}, cons[[2]] /. u -> # & /@ Table[u[i], {i, 0, hor - 1}]]


Then compute the controller. (This is a multiparametric QP problem and efficiently supported in 12.3)

controller = ArgMin[{cost1, And @@ cons1}, Table[u[i], {i, 0, hor - 1}]]


It's piecewise affine.

Plot[controller, {x[0], -1, 1}, PlotLegends -> Table[u[i], {i, 0, hor - 1}]]


Alternatively

As you have in your example, we can specify the stateEqCons explicitly as constraints and use the original expression for cost.

cons2 = Join[cons1, Apply[Equal, Flatten[stateEqCons], {1}]];
controller = ArgMin[{cost, And @@ cons2}, Join[Table[u[i], {i, 0, hor - 1}], Table[x[i], {i, hor}]]][[1 ;; hor]]


This will result in the same controller as before.