Simulating Minimax MPC (chapter 11 of PHD thesis)

[Florian Larcher]

Hello Mr. Löfberg,

i currently try to simulate your proposed Minimax MPC controller (chapter 11 of the Phd thesis) 'manually' via MATLAB/Simulink and YALMIP.

By 'manually' i mean, that i directly construct the robust performance LMI (11.15), p. 147 in terms of sdp variables and then solve the problem through SDPT3/YALMIP.

As a matter of simplicity, I do not consider input or state constraints so far.

The system to be controlled is a very simple 2. order LTI-system. The control objective is to bring the output to zero. (Y'QY+U'RU)

Further I know, that the performance of my code is very poor, because I could compute things offline, that i do online now, but the thing is, i want to understand everything first and get a working minimal example, before I optimize code.

The solver does solve the problem without any errors (problem=0), but the control output U is very small (~1e-20). The result is, that the system output nearly stays at it's defined initial value y(0)=1.

The question is now, did I just make a mistake in my implementation, or did I understand the theory completely wrong? 

I am also very unsure about the construction of the sdp-variable 'TauTilde' (see code below).

I have reduced the example to a minimal one and after initializing the init file, the model should work and produce the above described behaviour. (see attachement)

I hope, one can understand the things i tried to explain, if not, please give me feedback.

Many thank's in advance!

The init-file and the online code are attached and additionally copied here:

OFFLINE CODE: (modelInit.m)

%% init

clear all;

close all;

clc;

format shortE;

%% simulation parameters

tStart=0;      % [s] simulation start time

tEnd=1e-3;   % [s] simulation stop time         

%% sample time

Ts      = 10e-6; % [s] sample time

dtMax   = Ts/10; % [s] maximum step size for simulation

%% system description

n = 2;          % system order

ka = 0.0001;    % system parameter 1

kb = 0.0001;    % system parameter 2

A = [-kb/ka 0; 1 0];    % continuous system matrix

B = [1/ka ; 0];         % continuous input matrix

G = [1 -1/ka; 0 0];     % continuous 'virtual input' matrix (LFT-form)

C = [0 1];              % output to be controlled

%% discretize

Ad = expm(A*Ts);

e = @(x) expm(A*x);

Bd = quadv(e,0,Ts)*B;

Gd = quadv(e,0,Ts)*G;

% discrete matrices:

A=Ad;

B=Bd;

G=Gd;

%% Minimax MPC (chapter 11)

N = 5;  % number of prediction steps

L = 2;  % dimension of delta-matrix (number of uncertain parameters)    

        

% scalings

s(1) = 0.1*1/ka; % 10% error for 1/ka

s(2) = 0.1*kb;   % 10% error for kb

% matrices for LFT-description

Dx = [ -kb*s(1) 0 ; s(2) 0 ];

Du = [s(1); 0];

Dp = [0 -s(1); 0 0];

% weights

dataRMPC.Qtilde = eye(N,N) * 1;

dataRMPC.Rtilde = eye(N,N) * 0.1;  

Asum = kron(eye(N-1,N-1),A);

Bsum = kron(eye(N-1,N-1),B);

Gsum = kron(eye(N-1,N-1),G);

% definitions on p.145 (eq.11.4a)

Atilde = [  zeros( n,(N-1)*n ), zeros( n,       n );

            Asum,               zeros( (N-1)*n, n )      

        ];

Btilde = [  zeros( n,(N-1)   ), zeros( n,       1 );

            Bsum,               zeros( (N-1)*n, 1 )     

        ];

Gtilde = [  zeros( n,(N-1)*L),  zeros( n,       L );

            Gsum,               zeros( (N-1)*n, L )      

        ]; 

% definitions on p. 145 (eq. 11.4b)

Ctilde  = kron(eye(N,N),C);

DxTilde = kron(eye(N,N),Dx);

DpTilde = kron(eye(N,N),Dp);

DuTilde = kron(eye(N,N),Du);

%% passing data to simulink

dataRMPC.n          = n;

dataRMPC.N          = N;

dataRMPC.L          = L;

dataRMPC.Atilde     = Atilde;

dataRMPC.Btilde     = Btilde;

dataRMPC.Ctilde     = Ctilde;

dataRMPC.DuTilde    = DuTilde;

dataRMPC.DxTilde    = DxTilde;

dataRMPC.Omega      = DxTilde * inv(eye(size(Atilde))-Atilde) * Gtilde + DpTilde;

dataRMPC.Lambda     = inv(eye(size(Atilde))-Atilde) * Gtilde;

%% system for simulation

testSystem = ss(A,B,eye(n,n),0,Ts);

x0 = [0;1];  % initial state

%% simulink bus

clear slBus1;

Simulink.Bus.createObject(dataRMPC);

%% simulate

open('model');

sim('model');

ONLINE CODE (runRMPC.m):

function [ uOpt, problem, solvertime ] = runRMPC(x,dataRMPC)

    %% interface

    n       = dataRMPC.n;

    N       = dataRMPC.N;

    L       = dataRMPC.L;

    Btilde  = dataRMPC.Btilde;

    Ctilde  = dataRMPC.Ctilde;

    DuTilde = dataRMPC.DuTilde;

    DxTilde = dataRMPC.DxTilde;

    Atilde = dataRMPC.Atilde;

    Lambda  = dataRMPC.Lambda;

    Omega   = dataRMPC.Omega;

    Qtilde  = dataRMPC.Qtilde;

    Rtilde  = dataRMPC.Rtilde; 

    

    %% ensure x to be a column vector

    size_x=size(x);

    if (size_x(2)~=1)

        x=x';

    else

        x=x;

    end

        

    %% problem description

    

    % variables

    t           = sdpvar(1,1);

    U           = sdpvar(N,1);

    tau         = sdpvar(N,1);

    TauTilde    = sdpvar(0,0);

    % definitions

    b       = [vec2col(x); zeros( (N-1)*n,1 ) ];    % (eq. 11.4c)

    Psi     = DxTilde * inv(eye(size(Atilde))-Atilde) * (Btilde*U + b) + DuTilde*U; % (eq. 11.10c)

    Xtilde  = inv(eye(size(Atilde))-Atilde) * (Btilde*U + b);  % (eq. 11.10a)

    

    % very unsure about this:

    for i=1:N

        zerosBefore = (i-1)*L;

        zerosAfter  = N*L-L-zerosBefore;

        matrix      = tau(i)*eye(L,L);

        currentLine = [ zeros(L,zerosBefore) matrix zeros(L,zerosAfter) ]; 

        TauTilde    = [ TauTilde; currentLine ];

    end

    Stilde = TauTilde;

    %sdisplay(TauTilde)

    

    % robust performance constraint

    Mconstr = [ t               (Ctilde*Xtilde)'                                    U'              Psi'                         ;

                Ctilde*Xtilde   inv(Qtilde)-Ctilde*Lambda*Stilde*Lambda'*Ctilde'    zeros(N,N)      -Ctilde*Lambda*Stilde*Omega' ;

                U               zeros(N,N)                                          inv(Rtilde)     zeros(N,N*L)                 ;

                Psi             -Omega*Stilde*Lambda'*Ctilde'                       zeros(N*L,N)    TauTilde-Omega*Stilde*Omega' ];

    constr1 = [ Mconstr >= 0 ];

   

    % solve problem

    options=sdpsettings('solver','sdpt3','verbose', 0); 

    diag=solvesdp(constr1,t,options)

    problem=diag.problem;

    solvertime=diag.solvertime;

    U = double(U)

    t = double(t)

    tau =double(tau)

    % extract first solution

    uOpt = U(1);

   

end

Many thank's again!!

  

[Johan Löfberg]

You have a system where you have essentially no control authority on the second state, so when the first state is 0, the second state remains constant unless you allow insanely large inputs (as the gain in B is 1e-7)

x = [0;1];

u = sdpvar(N,1);

obj = 0;

for i = 1:N

    x = A*x + B*u(i);

    obj = obj + x'*x+u(i)^2;

end

solvesdp([],obj)

double(u)

adding to that 

1. 10 years later, I can happily say that I would never use the proposed method as you probably don't want to solve SDPs on-line

2. For faster simulation, try to avoid to redefine the model in every sample, see post tagged simulink

3. For faster simulation, Mosek is probably a better choice, since the startup overhead is much lower (which in combination with the optimizer approach can make a huge difference)

[Florian Larcher]

Thank's for your fast reply!

Points 2+3 are great advises - I will take them into account later!
Point 1: Which method would you use now, if you wanted to control (model predictive) a system with uncertain A and B matrix?

Small gain in the B-matrix:
You are right, I artificially increased the gain in the B-matrix and altered the initial state - unfortunately the result was similar - sometimes the solver returned infeasibility.

So, I'm afraid, something is wrong in my implementation.
I'm very unsure about the construction of the problem description and especially the variable TauTilde and Stilde.
Maybe you could be so kind and take again a look at the corresponding code:

    % variables

    t           = sdpvar(1,1);

    U           = sdpvar(N,1);

    tau         = sdpvar(N,1);

    TauTilde    = sdpvar(0,0);

    % definitions

    b       = [vec2col(x); zeros( (N-1)*n,1 ) ];    % (eq. 11.4c)

    Psi     = DxTilde * inv(eye(size(Atilde))-Atilde) * (Btilde*U + b) + DuTilde*U; % (eq. 11.10c)

    Xtilde  = inv(eye(size(Atilde))-Atilde) * (Btilde*U + b);  % (eq. 11.10a)

    

    % very unsure about this (and very unefficient):

[Johan Löfberg]

SDPT3 complains due to the horrible numerics in the problem. I guess any other solver would do too.

Look at the model you have

> pole(ss(A,B,C,0))

ans =

     0
    -1

>> tzero(ss(A,B,C,0))

ans =

   Empty matrix: 0-by-1

You have a simple first-order system with an integrator and a pole in -1. A reasonable sampling-time would be some fractions of a a second I guess. Say 0.1 seconds. You use 1e-5. Nothing happens to the second state during that time-span

You will not get any reasonable results until you've come up with a sensible model. Before you do robust MPC, get unconstrained nominal MPC to work (of course no solver needed, but it is the basis for everything else in MPC)

  U           = sdpvar(N,1);

    obj = 0;
    for i = 1:

N
        obj = obj + x'*x+0.1*U(i)*U(i);
    end    
    solvesdp([],obj);
    uOpt = double(U(1));
    problem = 0;
    solvertime = 0;

[Florian Larcher]

OK, I will prepare a realistic model, which we were able to control via standard MPC already. This was only possible after normalizing the system description. I guess, I have to do so for the RMPC approach.
I will post that example here - probably next week when I have time.

Thank you so far, Mr. Löfberg! 

[Florian Larcher]

OK, finally I had time to prepare a new model. It's a real existing system (very small two-mass-system with DC-motor) on which we want to analyze RMPC.

My workflow was as follows:
1) construct a continuous LFT-model based on the signal flow graph with possible uncertainty in each parameter (9 parameters)

2) discretize the LFT model via the control system toolbox (zoh)

3) normalize the discrete LFT matrices

4) set up the optimizer object offline (RMPC.init)

5) solve the problem online in a receding horizon manner (runRMPC.m)

The result is, that I am able to control the system output with respect to a reference, but only when there is NO uncertainty (Dx=Du=Dp=0). The behavior is similar compared to standard MPC, which I think, is a good point to start with.

The "problem" begins, when uncertainty is added (i.e. 5% or 1% uncertainty in each parameter) (Dx, Du, Dp != 0). The control output is zero for all times, the solver reports problem=0.

Now, several questions occur:
1) Is the discretization of the continuous LFT-model correct? (the continuous model seems to be correct, since I have compared it against the corresponding perturbed system)

2) Is the normalization of the discrete LFT-model adequate with respect to the sdp solver?

3) Is the way, I generate the problem description (RMPCinit.m) wrong, or is the semidefinite relaxation just so bad for this "big uncertainty range"?

4) Can I modify the problem description to consider only a "fixed uncertainty" (\Delta_k = const.) during the time horizon? I guess this would yield to a less conservative behavior, because the method does not have to take into account this "huge range of possible uncertainty realizations". Also this would be a more realistic assumption for our application.

I hope, this was not too long and not too many questions - many thank's in advance!

I have also attached all files in a *.zip. 

offline code: (RMPCinit.m)

%% init

clear all;

close all;

clc;

format shortE;

%% simulation parameters

tStart=0;    % [s] simulation start time

tEnd=8e-4;   % [s] simulation stop time         

%% sample times

Ts      = 10e-6; % [s] sample time

dtMax   = Ts/10; % [s] maximum step size for simulation

%% reference signal

load yRef;

%% initial state

x0 = [0 0 0 0 0]';

    

%% general data

N = 10;     % number of prediction steps

n = 5;      % system order

L = 9;      % number of uncertainties / dimension of square delta-matrix

%% weights 

Qtilde = eye(N,N) * 1;      

Rtilde = eye(N,N) * 0.1;    

%% LFT-model

% load data 

load LFTdataZero;   % 0% uncertainty

%load LFTdataFive;   % 5% uncertainty in each parameter

% continuous LFT matrices

Ac  =LFTdata.Ac;

Bc  =LFTdata.Bc;

Gc  =LFTdata.Gc;

C   =LFTdata.C;

Dx  =LFTdata.Dx;

Du  =LFTdata.Du;

Dp  =LFTdata.Dp;

% discretization of LFT model

sysc = ss(Ac,[Bc Gc],zeros(1,n),0);

sysd = c2d(sysc,Ts,'zoh');

Ad = sysd.a;

tempB  = sysd.b;

Bd = tempB(:,1);

Gd = tempB(:,2:end);

% try to normalize (discrete) system matrices

Sx = diag([ 2.0000e+00, 1.5625e-02, 1.2800e+02, 1.5625e-02, 1.2800e+02 ]);  % computed via prescale.m

Su = 1e5;

Sp = diag([1e7 1e3 1e0 1e3 1e6 1e7 1e-1 1e-1 1e0]);

                

SxInv = inv(Sx);

SuInv = inv(Su);

SpInv = inv(Sp);

Ad  = SxInv*Ad*Sx;

Bd  = SxInv*Bd*Su;

Gd  = SxInv*Gd*Sp;

C   = C*Sx;

Dx  = Dx*Sx;

Du  = Du*Su;

Dp  = Dp*Sp;

%% problem description 

yalmip('clear');

t           = sdpvar(1,1);

x           = sdpvar(n,1);

U           = sdpvar(N,1);

yRefN       = sdpvar(N,1);

tau         = sdpvar(N,1);

TauTilde    = sdpvar(0,0);

Asum    = kron(eye(N-1,N-1),Ad);

Bsum    = kron(eye(N-1,N-1),Bd);

Gsum    = kron(eye(N-1,N-1),Gd);

Ctilde  = kron(eye(N,N),C);

% eq. (11.4)

Atilde = [  zeros( n,(N-1)*n ), zeros( n,       n );

            Asum,               zeros( (N-1)*n, n )      

        ];

Btilde = [  zeros( n,(N-1)   ), zeros( n,       1 );

            Bsum,               zeros( (N-1)*n, 1 )     

        ];

Gtilde = [  zeros( n,(N-1)*L),  zeros( n,       L );

            Gsum,               zeros( (N-1)*n, L )      

        ]; 

DxTilde = kron(eye(N,N),Dx);

DpTilde = kron(eye(N,N),Dp);

DuTilde = kron(eye(N,N),Du);    

b       = [x; zeros( (N-1)*n,1 ) ]; 

% eq. (11.10)

Xtilde  = inv(eye(size(Atilde))-Atilde) * (Btilde*U + b);

Lambda  = inv(eye(size(Atilde))-Atilde) * Gtilde;                    

Psi     = DxTilde * inv(eye(size(Atilde))-Atilde) * (Btilde*U + b) + DuTilde*U; 

Omega   = DxTilde * inv(eye(size(Atilde))-Atilde) * Gtilde + DpTilde; 

for i=1:N

    zerosBefore = (i-1)*L;

    zerosAfter  = N*L-L-zerosBefore;

    matrix      = tau(i)*eye(L,L);

    currentLine = [ zeros(L,zerosBefore) matrix zeros(L,zerosAfter) ]; 

    TauTilde    = [ TauTilde; currentLine ];

end

Stilde = TauTilde;

% robust performance constraint

Mconstr = [ t                               (Ctilde*Xtilde)'-yRefN'                             U'              Psi'                         ;

            Ctilde*Xtilde-yRefN             inv(Qtilde)-Ctilde*Lambda*Stilde*Lambda'*Ctilde'    zeros(N,N)      -Ctilde*Lambda*Stilde*Omega' ;

            U                               zeros(N,N)                                          inv(Rtilde)     zeros(N,N*L)                 ;

            Psi                             -Omega*Stilde*Lambda'*Ctilde'                       zeros(N*L,N)    TauTilde-Omega*Stilde*Omega' ];

constr = [ Mconstr >= 0 ]; 

% input constraint

uMax = 30;

if (1)

    constr =  [ constr, U<=SuInv*ones(N,1)*uMax, U>=-SuInv*ones(N,1)*uMax ];

end

    

% optimizer object

solver = 2;

switch solver

    case 1

        options=sdpsettings('solver','lmilab'); 

    case 2

        options=sdpsettings('solver','sdpt3'); 

    case 3

        options=sdpsettings('solver','sedumi');

end

controllerRMPC = optimizer(constr,t,options,{x,yRefN},U(1));

% pass data to simulink

dataRMPC.N          = N;    

dataRMPC.yRef       = yRef;    

dataRMPC.SxInv      = SxInv;

dataRMPC.Su         = Su;

online code: (runRMPC.m)

function [ uOpt, problem, solvertime ] = runRMPC(xIn,k)

    

    %% get data from workspace

    controllerRMPC  = evalin('base','controllerRMPC');

    dataRMPC        = evalin('base','dataRMPC');

    N               = dataRMPC.N;

    yRef            = dataRMPC.yRef;

    kEnd            = length(yRef);

                  

    %% normalize state measurement

    xIn=dataRMPC.SxInv*xIn;

    %% building reference for prediction (moving horizon)

    kN = kEnd - N;

    if ( k < kN)

        yRefN = yRef(k:k+N-1)'; 

    else % in case of reaching the end of simulation, extend time horizon based on last known value

        yRefN = [ yRef(k:kEnd) yRef(kEnd)*ones(1,N-(kEnd-k)) ]';

    end

        

    %% solve SDP

    tic;

    [uOpt, problem] = controllerRMPC{{xIn,yRefN}};

    solvertime = toc;

    %% denormalize calculated control action

    uOpt = dataRMPC.Su*uOpt;    

end

   

[Johan Löfberg]

I am not going to be able to answer any of your detailed questions, but in general, it could very well be that it is optimal to do nothing for any non-zero uncertainty, or that it is optimal to do nothing when solving the conservative approximation of the minimax problem.