MPC Example
- Input-only constrained MPC (condensed form)
- Input- and state-constrained MPC (sparse form)

MPC Example

This example shows how to use FiOrdOs for MPC regulation problems of type

$\begin{align*} \min_{\substack{u_0\ldots u_{N-1}\\x_1\ldots x_N}}\quad & \tfrac{1}{2}x_N^T P x_N + \tfrac{1}{2}\sum_{k=0}^{N-1} x_k^T Q x_k + u_k^T R u_k \\ \textrm{s.t.}\quad &x_{k+1} = A x_k + B u_k\quad&& k=0,\ldots, N-1\\[1ex] & x_k \in \mathcal{X}\subseteq\mathbb{R}^{n_x} \quad&& k=1,\ldots, N-1\\ & x_N \in \mathcal{X}_f\subseteq\mathcal{X}\\ & u_k \in \mathcal{U}\subseteq\mathbb{R}^{n_u}\quad&& k=0,\ldots, N-1\ ,\\ \end{align*}$

where $x_0$ is the current state of the system.

Let us start with defining some (stable) system and MPC parameters.

A=[0.8 1; 0 0.9];
B=[-1;2];
[nx,nu]=size(B);
 
N=5;
Q=eye(nx);
R=eye(nu);

Input-only constrained MPC (condensed form)

Let us consider a problem with box constraints on the inputs

$\mathcal{U} = \left\{ u\in\mathbb{R}^{n_u} \left|\right. -0.7\leq u \leq 0.7 \right\}\ ,$

but no state constraints, i.e.

$\mathcal{X}=\mathbb{R}^n\ .$

To obtain a stable closed loop, we take P=dlyap(A',Q) and $\mathcal{X}_f=\mathbb{R}^n\ .$

The typical procedure is to eliminate the states via

$\underbrace{\begin{bmatrix} x_1 \\x_2\\\vdots \\x_N\\\end{bmatrix}}_{X} = \underbrace{\begin{bmatrix} A \\ A^2 \\ \vdots \\ A^N \end{bmatrix}}_{\mathcal{A}} x_0 + \underbrace{\begin{bmatrix} B & & & \\ A B & B & & \\ \vdots & \ddots & \ddots & \\ A^{N-1} B &\ldots & AB & B \end{bmatrix} }_{\mathcal{B}} \underbrace{\begin{bmatrix} u_0 \\u_1\\\vdots \\u_{N-1}\\\end{bmatrix}}_{U}\ ,$

such that the optimization problem can be written in condensed form (the decision variable is the sequence of inputs $U$ only) as

$\begin{align*} \min_{U}\quad&\tfrac{1}{2} U^T H U + g(x_0)^T U + c(x_0) \\ \textrm{s.t.}\quad & U\in\mathcal{U}^N \\ \end{align*}$

where

$\begin{align*} H &=(\mathcal{B}^T\mathcal{Q}\mathcal{B}+\mathcal{R})\ ,\quad g(x_0) = \mathcal{B}^T\mathcal{Q} \mathcal{A}x_0\ ,\quad c(x_0) = \tfrac{1}{2} x_0^T (\mathcal{A}^T\mathcal{Q}\mathcal{A} + Q) x_0\ ,\\ \mathcal{Q} &=\begin{bmatrix} Q & & & \\ &\ddots& & \\ & & Q & \\ & & & P \\ \end{bmatrix}\ ,\quad \mathcal{R}=\begin{bmatrix} R & & \\ &\ddots& \\ & & R \\ \end{bmatrix}\ . \end{align*}$

Using FiOrdOs syntax, this problem is specified by

umin=-0.7;
umax= 0.7;
 
P=dlyap(A',Q);
 
UU=SimpleSet(N);
UU.addSet(1:N,EssBox(nu,'l',umin,'u',umax));
 
Ai=A;
AA=Ai;
for i=2:N
    Ai=A*Ai;
    AA=[AA;Ai];
end
 
AiB=B;
BB=kron(eye(N),AiB);
for i=1:N-1;
    AiB=A*AiB;
    BB=BB+kron(diag(ones(N-i,1),-i),AiB);
end
 
QQ=blkdiag(kron(eye(N-1),Q),P);
RR=kron(eye(N),R);
H =(BB'*QQ*BB + RR);
 
op=OptProb('H',H, 'g','param', 'c','param', 'X',UU)

where g and c are defined parametric (param), since these entities depend on the initial state $x_0$ .

Next, we set up the solver, generate the code and compile the MEX-interface (requires a compiler to be installed and defined in Matlab).

s=Solver(op)
s.generateCode('prefix','mpc1_');
rehash;
mpc1_mex_make;

The generated solver can now be used in a closed-loop simulation.

Nsteps=20;
 
trajU=nan(nu,Nsteps);
trajX=nan(nx,Nsteps+1);
trajX(:,1)=[1;2];  % initial state
 
mparams=struct();
msetgs=struct();
msetgs.algo.maxit=50;
 
for k=1:Nsteps
    x0=trajX(:,k);
 
    % parametric data
    mparams.g=BB'*QQ*AA*x0;
    mparams.c=0.5*x0'*(AA'*QQ*AA + Q)*x0;
 
    % call solver
    mres=mpc1_mex(mparams,msetgs);
 
    % apply input u_0 to system
    trajU(:,k) = mres.x(1:nu);
    trajX(:,k+1) = A*trajX(:,k) + B*trajU(:,k);
end
 
% plot results
figure(1); clf; stairs(0:Nsteps-1,trajU'); title('inputs'); xlabel('steps');
figure(2); clf; plot(trajX(1,:),trajX(2,:),'*:'); title('state trajectory'); xlabel('x_1'); ylabel('x_2');

Tuning hints

You could for example try the following to possibly improve the performance of the solver.

Automatic Preconditioning
```
s.setAutoPrecond('algo')
```

Warmstarting

Use previous solution as warmstart for new solver call.

if k>1
msetgs.algo.init=mres.x;                         % unshifted or ...
msetgs.algo.init=[mres.x(nu+1:end);zeros(nu,1)]; % shifted or ...
end

Inline H
Write the matrix-vector multiplication involving element by element in C.
```
s.setSettings('approach','inlineH',true);
```
Stopping condition
Stop the algorithm when the function value is $\epsilon$ -suboptimal ( $f(x)-f^*\leq\epsilon$ )
```
s.setSettings('algo', 'stopg',true, 'stopgEps', stopgEps);
```
with stopgEps $= \sqrt{\tfrac{2\epsilon}{1/\mu-1/L}},$ where $\mu>0,L>0$ are the minimal and maximal eigenvalue of the (preconditioned) matrix . (reference)
Change maximum number of iterations

Input- and state-constrained MPC (sparse form)

Let us consider a problem with box constraints on the inputs and states

$\mathcal{U} = \left\{ u\in\mathbb{R}^{n_u} \left|\right. -0.7\leq u \leq 0.7 \right\}\ ,$

$\mathcal{X} = \left\{ x\in\mathbb{R}^{n_x} \left|\right. \bigl[\begin{smallmatrix}-3.5\\-0.5\end{smallmatrix}\bigr]\leq x \leq \bigl[\begin{smallmatrix}3.5\\2\end{smallmatrix}\bigr] \right\}\ .$

To obtain a stable closed loop, we take [K,P]=dlqr(A,B,Q,R) and $\mathcal{X}_f = \left\{ x\in\mathbb{R}^{n_x} | \tfrac{1}{2}x^T P x \leq \tfrac{1}{2}\gamma \right\}$ with $\gamma$ maximal such that $\mathcal{X}_f \subset \mathcal{X} \cap \{ x \in \mathbb{R}^{n_x} | -K x \in \mathcal{U} \}$ .

Because FiOrdOs does not support ellipsoids, we make a change of variables for the terminal state by $\tilde{x}_N = P^{1/2} x_N$ , such that $\mathcal{X}_f$ becomes the ball $\tilde{\mathcal{X}}_f = \bigl\{ \tilde{x}\in\mathbb{R}^{n_x} \left|\right. \tilde{x}^T \tilde{x} \leq \gamma=r^2 \bigr\}.$

Now, the optimization problem can be written as

$\begin{align*} \min_{Z}\quad&\tfrac{1}{2}Z^T H Z + c(x_0) \\ \textrm{s.t.}\quad & Z\in\mathcal{U}^N\times\mathcal{X}^{N-1}\times\tilde{\mathcal{X}}_f \\ & A_e Z = b_e \end{align*}$

where

$\begin{align*} Z&=\left[\begin{array}{c}u_0\\\vdots\\u_{N-1}\\\hline x_1\\\vdots\\x_{N-1}\\\tilde{x}_N\end{array}\right]\ ,\quad H = \left[\begin{array}{ccc|cccc} R & & & & & & \\ &\ddots& & & & & \\ & & R & & & & \\\hline & & & Q & & & \\ & & & &\ddots& & \\ & & & & & Q & \\ & & & & & & I \\ \end{array}\right]\ ,\quad c(x_0) = \tfrac{1}{2}x_0^T Q x_0 \ ,\quad\\ A_e &= \left[\begin{array}{ccccc|ccccccc} -B & & & & &I & & & & \\ &-B & & & &-A & I & & & \\ & &\ddots& & & &\ddots & \ddots & & \\ & & &-B & & & & -A & I & \\ & & & & -B & & & & -A & P^{-1/2} \end{array}\right]\ ,\quad b_e(x_0)= \left[\begin{array}{c} A\\ 0\\ \vdots\\ 0\\ 0 \end{array}\right] x_0 \ . \end{align*}$

Using FiOrdOs syntax, this problem is specified by

umin=-0.7;
umax= 0.7;
xmin=[-3.5;-0.5];
xmax=[3.5;2];
 
[K,P]=dlqr(A,B,Q,R);
gam=min([xmax;-xmin;umax;-umin].^2 ./ sum(([eye(nx);-eye(nx);-K;K]*P^(-1/2)).^2,2));
 
ZZ=SimpleSet(2*N);
ZZ.addSet(1:N,EssBox(nu,'l',umin,'u',umax));
ZZ.addSet(N+(1:N-1),EssBox(nx,'l',xmin,'u',xmax));
ZZ.addSet(N+(N),EssBall(nx,'r',sqrt(gam))); 
 
H =blkdiag(kron(eye(N),R),  kron(eye(N-1),Q),eye(size(P)));
 
Aeu=kron(eye(N),-B);
Aex=blkdiag(eye((N-1)*nx),P^(-1/2)) - kron(diag(ones(N-1,1),-1),A);
Ae =[Aeu,Aex];
 
op=OptProb('H',H, 'c','param', 'X',ZZ, 'Ae',Ae, 'be','param')

where c and be are defined parametric (param), since these entities depend on the initial state $x_0$ .

Next, we set up the solver, generate the code and compile the MEX-interface. Note that by default, FiOrdOs chooses the primal-dual approach for this problem.

s=Solver(op)
s.generateCode('prefix','mpc2_');
rehash;
mpc2_mex_make;

The generated solver can now be used in a closed-loop simulation.

Nsteps = 20;
 
trajU=nan(nu,Nsteps);
trajX=nan(nx,Nsteps+1);
trajX(:,1)=[1;2];  % initial state
 
mparams=struct();
msetgs=struct();
msetgs.approach.apprMaxit=200;
 
for k=1:Nsteps
    x0=trajX(:,k);
 
    % parametric data
    mparams.c=0.5*x0'*Q*x0;
    mparams.be =[A;zeros((N-1)*nx,nx)]*x0;
 
    % call solver
    mres=mpc2_mex(mparams,msetgs);
 
    % apply input u_0 to system
    trajU(:,k) = mres.x(1:nu);
    trajX(:,k+1) = A*trajX(:,k) + B*trajU(:,k);
end
 
% plot results
figure(3); clf; stairs(0:Nsteps-1,trajU'); title('inputs'); xlabel('steps');
figure(4); clf; plot(trajX(1,:),trajX(2,:),'*:'); title('state trajectory'); xlabel('x_1'); ylabel('x_2');

Tuning hints

You could for example try the following to possibly improve the performance of the solver.

Warmstarting

Use previous solution as warmstart for new solver call.

if k>1
    msetgs.approach.apprInitX  = mres.x;                  
    msetgs.approach.apprInitLa = mres.la;
end

Inline A
Write the matrix-vector multiplications involving element by element in C.
```
s.setSettings('approach', 'inlineA',true);
```
Stopping criterion instead of fixed iteration number
Stop the approach whenever the gradient-based stopping criterion is true:
```
s.setSettings('approach', 'stopg',true, 'stopgEpsPrimal',1e-3, 'stopgEpsDual',1e-3);
```
Balance primal/dual step size
The primal-dual approach has a tuning parameter 'balancePrimalDual'. If this parameter is >1, more emphasis is put on dual progress whereas if it is <1, primal convergence is emphasized. A reasonable (positive) value can be found empirically, e.g.
```
s.setSettings('approach', 'balancePrimalDual', 1.1);
```
Reduce the maximum number of iterations
Check by inspection if a maximum of 100 iterations still gives acceptable control performance.
```
msetgs.approach.apprMaxit = 100;
```

Last modified: 2019/08/13 16:51

Driven by DokuWiki