Full Code of HiroIshida/robust-tube-mpc for AI

Repository: HiroIshida/robust-tube-mpc
Branch: master
Commit: 427a181dd368
Files: 18
Total size: 29.9 KB

Directory structure:
gitextract_gc5y9v0w/

├── .gitignore
├── LICENSE.md
├── README.md
├── example/
│   ├── example_MPC.m
│   ├── example_dist_inv_set.m
│   ├── example_optimalcontrol.m
│   └── example_tubeMPC.m
├── note/
│   ├── .gitignore
│   └── idea.tex
└── src/
    ├── ConstraintManager.m
    ├── DisturbanceLinearSystem.m
    ├── Graphics.m
    ├── LinearSystem.m
    ├── ModelPredictiveControl.m
    ├── OptimalControler.m
    ├── TubeModelPredictiveControl.m
    └── utils/
        ├── convert_Poly2Mat.m
        └── number2string.m

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitignore
================================================
old/
*.db
*.asv
example/results/

pathdef.m
/tbxmanager


================================================
FILE: LICENSE.md
================================================
MIT License

Copyright (c) 2018 Hirokazu Ishida

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.


================================================
FILE: README.md
================================================
# Robust Model Predictive Control Using Tube
This repository includes examples for the tube model predictive control (tube-MPC)[1] as well as the generic model predictive control (MPC) written in MATLAB.

## Requirement

1) optimization_toolbox (matlab)<br>
2) control_toolbox (matlab)<br>
3) Multi-Parametric Toolbox 3 (open-source and freely available at http://people.ee.ethz.ch/~mpt/3/)

## Feedback, bug reports, contributions
If you find this package helpful, giving a "star" to this repositry will be a happy feedback for me! If you find a bug, or have more broader kind of quession about tube MPC,please post that in the [issue page](https://github.com/HiroIshida/robust-tube-mpc/issues). I will try hard to respond to questions via e-mail but, I **strongly recommend** do it in the issue page. It's much easier for me to keep myself on track.

## Usage
See `example/example_tubeMPC.m` and `example/example_MPC.m` for the tube-MPC and generic MPC, respectively. Note that every inequality constraint here is expressed as a convex set. For example, the constraints on the state `Xc` is specified as a rectangular, which is constructed with 4 vertexes. When considering a 1-dim input `Uc`, `Uc` will be specified by min and max value (i.e. `u∊[u_min, u_max]`), so it will be constructed by 2 vertexes. For more detail, please see the example codes.

## Short introduction to the tube MPC
After running `example/example_tubeMPC.m`, you will get the following figure sequence.
![the gif file](/fig/tube_mpc.gif)

Now that you can see that the green nominal trajectory starting from the bottom left of the figure and surrounding a "tube". At each time step, the nominal trajectory (green line) is computed online. 

Let me give some important details. The red region `Xc` that contains the pink region `Xc-Z` is the state constraint that we give first. However, considering the uncertainty, the tube-MPC designs the nominal trajectory to be located inside `Xc-Z`, which enables to put "tube" around the nominal trajectory such that the tube is also contained in `Xc-Z`. Of course, the input sequence associated with the nominal trajectory is inside of `Uc-KZ`. 

## Disturbance invariant set
I think one may get stuck at computation of what paper [1] called "disturbance invariant set". The disturbance invariant set is an infinite [Minkowski addition](https://en.wikipedia.org/wiki/Minkowski_addition) `Z = W ⨁ Ak*W ⨁ Ak^2*W...`, where ⨁ denotes Minkowski addition. Because it's an infinite sum of Minkowski addition, computing Z analytically is intractable. In [2], Racovic proposed a method to efficiently compute an outer approximiation of Z, which seems to be heavily used in MPC community. In this repository, computation of Z takes place in the constructor of `DisturbanceLinearSystem` class. To understand how Z guarantee the robustness, running `example/example_dist_inv_set.m` may help you.

## Maximum positively invariant set
I used the maximal positively invariant (MPI) set `Xmpi` as the terminal constraint set. (Terminal constraint is usually denoted as Xf in literature). Book [3] explains the concept of the MPI and algorithm well in section 2.4. `Xmpi` is computed in the constructor of `OptimalControler.m`. Note that the MPI set is computed with `Xc` and `Uc` in the normal MPC setting, but in the tube-MPC the MPI set is computed with `Xc⊖Z`and `Uc⊖Z` instead.

# Reference
[1] Mayne, David Q., María M. Seron, and S. V. Raković. "Robust model predictive control of constrained linear systems with bounded disturbances." Automatica 41.2 (2005): 219-224.
[2] Rakovic, Sasa V., et al. "Invariant approximations of the minimal robust positively invariant set." IEEE Transactions on Automatic Control 50.3 (2005): 406-410.
[3] Kouvaritakis, Basil, and Mark Cannon. "Model predictive control." Switzerland: Springer International Publishing (2016).


================================================
FILE: example/example_MPC.m
================================================
addpath('../src/')
addpath('../src/utils/')

%the usage is mostly same as tubeMPC
A = [1 1; 0 1];
B = [0.5; 1]; 
Q = diag([1, 1]);
R = 0.1;
mysys = LinearSystem(A, B, Q, R);

Xc_vertex = [2, -2; 2 2; -10 2; -10 -2];
Uc_vertex = [1; -1];
Xc = Polyhedron(Xc_vertex);
Uc = Polyhedron(Uc_vertex);

% Unlike tube-MPC, this generic MPC doesn't guarantee the robustness. 
% In the following simulation you will notice that the generated path almost touch the boundary.
% Thus, you can imagine that if some additive disturbance is considered, then the state can 
% easily be off the boundary, and the succeeding optimization will no longer be feasible.
% Please add some noise and do experiments.
N_horizon = 5;
mpc = ModelPredictiveControl(mysys, Xc, Uc, N_horizon);

x = [-7; -2];
savedir_name = 'results';
for i = 1:15
    u_next = mpc.solve(x);
    x = mysys.propagate(x, u_next); % + add some noise here
    mpc.show_prediction();
    pause(0.1);
    clf;
end



================================================
FILE: example/example_dist_inv_set.m
================================================
addpath('../src/')
addpath('../src/utils/')

% specify your own discrete linear system
A = [1 1; 0 1];
B = [0.5; 1]; 
Q = diag([1, 1]);
R = 0.1;

% construct a convex set of system noise (2dim here)
W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15];
W = Polyhedron(W_vertex);

% construct disturbance Linear system
% note that disturbance invariant set Z is computed and stored as member variable in the constructor.
disturbance_system = DisturbanceLinearSystem(A, B, Q, R, W); 

% you can see that with any disturbance bounded by W, the state is guaranteed to inside Z
x = zeros(2); % initial state 
for i = 1:50
    u = disturbance_system.K * (x - 0);
    x = disturbance_system.propagate(x, u); % disturbance is considered inside the method
    clf;
    Graphics.show_convex(disturbance_system.Z, 'g', 'FaceAlpha', .3); % show Z
    scatter(x(1, :), x(2, :)); % show particle
    pause(0.01);
end


================================================
FILE: example/example_optimalcontrol.m
================================================
addpath('../src/')
addpath('../src/utils/')

% make your own discrete linear system
A = [1 1; 0 1];
B = [0.5; 1]; 
Q = diag([1, 1]);
R = 0.1;
mysys = LinearSystem(A, B, Q, R); 

% constraints on 2dim state Xc and 1dim input Uc
Xc_vertex = [2, -2; 2 2; -10 2; -10 -2];
Uc_vertex = [1; -1];
Xc = Polyhedron(Xc_vertex);
Uc = Polyhedron(Uc_vertex);

% create a optimal controler
% if N_horizon is too small, the path will never reach inside 
% the maximum positively invariant set (X_MPI) in time step N_horizon, 
% then the problem becomes infeasible.
% OptimalControler.solve method returns optimal path (x_seq) and input (u_seq)
N_horizon = 20; 
optcon = OptimalControler(mysys, Xc, Uc, 20);
x_init = [-7; -2];
optcon.add_initial_eq_constraint(x_init);
[x_seq, u_seq] = optcon.solve();


================================================
FILE: example/example_tubeMPC.m
================================================
addpath('../src/')
addpath('../src/utils/')

% fix random seed
rng(0);

% make your own discrete linear system with disturbance
A = [1 1; 0 1];
B = [0.5; 1]; 
Q = diag([1, 1]);
R = 0.1;

W_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15]; % construct a convex set of disturbance (2dim here)
W = Polyhedron(W_vertex);

% construct disturbance Linear system
disturbance_system = DisturbanceLinearSystem(A, B, Q, R, W);

% constraints on state Xc and input Uc
Xc_vertex = [2, -2; 2 2; -10 2; -10 -2];
Uc_vertex = [1; -1];
Xc = Polyhedron(Xc_vertex);
Uc = Polyhedron(Uc_vertex);

% create a tube_mpc simulater
% if N_horizon is too small, the path will never reach inside the robust MPI-set X_mpi_robust in time step N_horizon, then the problem becomes infeasible. 
N_horizon = 10;
w_min = [0; -0.10];
w_max = [0; 0.10];
mpc = TubeModelPredictiveControl(disturbance_system, Xc, Uc, N_horizon);

% The robust MPC guidances the path inside the robust MPI-set so that the path will reach the robust MPI-set in N_horizon. 
x = [-7; -2];
savedir_name = './results/';
mkdir(savedir_name);

for i = 1:15
    disp(i)
    u_next = mpc.solve(x);
    x = disturbance_system.propagate(x, u_next); % additive disturbance is considered inside the method 
    mpc.show_prediction();
    filename = strcat(savedir_name, 'tmpc_seq', number2string(i), '.png')
    saveas(gcf, char(filename)); % removing this line makes the code much faster
    clf;
end



================================================
FILE: note/.gitignore
================================================
*.aux
*.dvi
*.log
*.out



================================================
FILE: note/idea.tex
================================================
\documentclass[15pt]{article}
\usepackage{cancel}
\usepackage{amsmath}
\usepackage{listings}
\usepackage{amsthm}
\usepackage{amssymb} %symbols for mapping
\usepackage{amsfonts}
\usepackage{mathptmx}
\usepackage{bm}
\usepackage{framed}
\usepackage{geometry}
\geometry{left=20mm,right=20mm,top=20mm,bottom=20mm}
\usepackage{hyperref}
\usepackage{mathtools}
\usepackage{color}
\usepackage{comment}
\usepackage{hyperref}
\newcommand{\argmax}{\mathop{\rm argmax}\limits}
\newcommand{\argmin}{\mathop{\rm argmin}\limits}
\newcommand{\mymax}{\mathop{\rm max}\limits}
\newcommand{\mymin}{\mathop{\rm min}\limits}
\newcommand{\np}{\mathcal{N}_p}

\definecolor{dkgreen}{rgb}{0,0.6,0}
\definecolor{gray}{rgb}{0.5,0.5,0.5}
\definecolor{mauve}{rgb}{0.58,0,0.82}

\lstset{frame=tb,
  language=matlab,
  aboveskip=3mm,
  belowskip=3mm,
  showstringspaces=false,
  columns=flexible,
  basicstyle={\small\ttfamily},
  numbers=none,
  numberstyle=\tiny\color{gray},
  keywordstyle=\color{blue},
  commentstyle=\color{dkgreen},
  stringstyle=\color{mauve},
  breaklines=true,
  breakatwhitespace=true,
  tabsize=3
}

\hypersetup{
  urlcolor=cyan
}
\begin{document}
\begin{center}
  \textbf{Possible extension to distributed MPC} 
\end{center}

Let just consider $x_p$ and $x_q$ at a particular time step. For simplicity, let me remove the time index and the bar notation. Also, 
\begin{align}
  \sum_{q\in \np}(x_q -x_p)^T Q (x_q - x_p) &= \sum_{q \in \np} \left( x_p^T Q x_p + - 2 x_q^T Q x_p + x_q^T Q x_q  \right) \\
   & = (\#\np) x_p^T Q x_p - 2 \left[ \sum_{q\in \np} x_q^T Q \right] ^T x_p + \sum_{q\in \np} x_q^T Q x_q
\end{align}
Letting $A = (\#\np) Q$, $b = -2\left[ \sum_{q\in \np} x_q^T Q \right] ^T$ and $c = \sum_{q\in \np} x_q^T Q x_q$, we now have the form of 
\[
  x_p^TAx_p + b x_p + c
\]
We have $N$ (number of waypoints of trajectory) of this. Note that you can do the same thing for $P$(for terminal cost) to make $A_f, b_f, c_f$. But for simplicity, let us just consider $P=Q$. Let me denote $X:=[x_{p_1}^T, \ldots x_{p_N}^T]^T$ (the path) and input sequence $U:=[u_{p_1}^T, \ldots, u_{p_N}^T]^T$. Also, let me write $S:=[X^T, U^T]^T$. Then the total objective function $J(X, U)$ will be 
\begin{align}
  J(X, U) &= \sum_{k\in [N]} \left( x_{p_k}^T A x_{p_k} + b x_{p_k} + c \right) + \sum_{k\in[N]} u_{p_k}^TQu_{p_{k}} \\
  &= \left( X^T \tilde{A} X + \tilde{b} X + \tilde{c} \right)  + U^T \tilde{R} U \\
  &= S^T \mathrm{diag}(\tilde{A}, \tilde{R}) S + [\tilde{b}, \mathrm{zeros}]^T S + \tilde{c}\\
\end{align} 
wher $\tilde{A} = \mathrm{diag}(A, \ldots, A)$, $\tilde{B} = [b, \ldots, b]$, $\tilde{c}=\sum_{i\in [N]}^{} c$ and $\tilde{R} = \mathrm{diag}(R, \ldots, R)$. 

In my original implementation, the cost $J$ is expressed by $S^T H S$ and $H$ is constructed by the following:
\begin{lstlisting}
function H = construct_costfunction(obj)
    % compute H
    Q_block = [];
    R_block = [];
    for itr=1:obj.N
        Q_block = blkdiag(Q_block, obj.sys.Q);
        R_block = blkdiag(R_block, obj.sys.R);
    end
    H = blkdiag(Q_block, obj.sys.P, R_block);
end
\end{lstlisting}
So first you should replace Q\_block and R\_block in final line by $\tilde{Q}$ and $\tilde{R}$. As for $obj.sys.P$ you can do the same. Note that in this setting, we have $[\tilde{b}, \mathrm{zeros}]^TS$ and $tilde{c}$. So, just compute then in the constructor when you make `OptimalControler` instance and then pass them to `quadprog` (MATLAB's quadprog accept the form of $x^T A x + b^Tx$).
\end{document}


================================================
FILE: src/ConstraintManager.m
================================================
classdef ConstraintManager < handle

    properties (SetAccess = private)
        C_equality;
        C_inequality;
    end

    methods (Access = public)

        function obj = ConstraintManager()
            obj.C_equality = containers.Map();
            obj.C_inequality = containers.Map();
        end

        function add_eq_constraint(obj, key, C_eq1, C_eq2)
            obj.C_equality(key) =  {C_eq1, C_eq2};
        end

        function add_ineq_constraint(obj, key, C_ineq1, C_ineq2)
            obj.C_inequality(key) =  {C_ineq1, C_ineq2};
        end

        function remove_eq_constraint(obj, key)
            remove(obj.C_equality, key);
        end

        function remove_ineq_constraint(obj, key)
            remove(obj.C_inequality, key);
        end

        function [C_eq1, C_eq2] = combine_all_eq_constraints(obj)
            C_eq_array = values(obj.C_equality);
            C_eq1 = [];
            C_eq2 = [];
            for i = 1:numel(C_eq_array)
                C_eq1 = [C_eq1; C_eq_array{i}{1}];
                C_eq2 = [C_eq2; C_eq_array{i}{2}];
            end
        end

        function [C_ineq1, C_ineq2] = combine_all_ineq_constraints(obj)
            C_ineq_array = values(obj.C_inequality);
            C_ineq1 = [];
            C_ineq2 = [];
            for i = 1:numel(C_ineq_array)
                C_ineq1 = [C_ineq1; C_ineq_array{i}{1}];
                C_ineq2 = [C_ineq2; C_ineq_array{i}{2}];
            end
        end
    end
end



================================================
FILE: src/DisturbanceLinearSystem.m
================================================
classdef DisturbanceLinearSystem < LinearSystem

    properties (SetAccess = private)
        W % convex set of distrubance
        Z % disturbance invariant set
    end

    methods (Access = public)

        function obj = DisturbanceLinearSystem(A, B, Q, R, W)
            obj = obj@LinearSystem(A, B, Q, R);

            obj.W = W;
            obj.Z = obj.compute_mrpi_set(1e-4);
        end

        function x_new = propagate(obj, x, u)
            w = obj.pick_random_disturbance();
            x_new = propagate@LinearSystem(obj, x, u) + w;
        end

    end

    methods (Access = public)

        function w = pick_random_disturbance(obj)
            % pick disturbance form uniform distribution
            verts = obj.W.V;
            b_max = max(verts)';
            b_min = min(verts)';

            % generate random until it will be inside of W
            while true
                w = rand(obj.nx, 1) .* (b_max - b_min) + b_min; 
                if obj.W.contains(w)
                    break
                end
            end
        end

        function Fs = compute_mrpi_set(obj, epsilon)
            % Computes an invariant approximation of the minimal robust positively
            % invariant set for 
            % x^{+} = Ax + w with w \in W
            % according to Algorithm 1 in 'Invariant approximations of
            % the minimal robust positively invariant set' by Rakovic et al. 
            % Requires a matrix A, a Polytope W, and a tolerance 'epsilon'.  
            [nx,~] = size(obj.Ak); 
            s = 0; 
            alpha = 1000;
            Ms = 1000;
            E = eye(nx);
            it = 0;
            while(alpha > epsilon/(epsilon + Ms))
                s = s+1;
                alpha = max(obj.W.support(obj.Ak^s*(obj.W.A)')./obj.W.b);
                mss = zeros(2*nx,1);
                for i = 1:s
                    mss = mss+obj.W.support([obj.Ak^i, -obj.Ak^i]);
                end
                Ms = max(mss);
                it = it+1;
            end

            Fs = obj.W;
            for i =1:s-1
                Fs = Fs+obj.Ak^i*obj.W;
            end
            Fs = (1/(1-alpha))*Fs;
        end

    end
end


================================================
FILE: src/Graphics.m
================================================
classdef Graphics<handle
    % This is a function collection rather than a class
    % So, All methods are static.
    % plot trajectory and show convex set which are projected from n-dim
    
    methods (Static)
        function show_convex(P, varargin)
            P_reduced = projectPolytope2Plane(P);
            fill(P_reduced.V(:, 1), P_reduced.V(:, 2), varargin{:})
            hold on;
        end

        function show_trajectory(x_seq, varargin)
            plot(x_seq(1, :), x_seq(2, :), varargin{:})
            hold on;
        end

    end
end

function P_projected = projectPolytope2Plane(P)
    vert = P.V;
    x_vert = round(vert(:, 1), 5);
    y_vert = round(vert(:, 2), 5);
    idx = convhull(x_vert, y_vert);
    P_projected = Polyhedron([x_vert(idx), y_vert(idx)]);
end


================================================
FILE: src/LinearSystem.m
================================================
classdef LinearSystem < handle
    properties (SetAccess = private)
        % common notation for linear system
        A; B; % dynamics x[k+1] = A*x[k]+B*u[k]
        nx; nu; % dim of state space and input space
        Q; R; % quadratic stage cost for LQR
        K; % LQR feedback coefficient vector: u=Kx
        P; % optimal cost function of LQR is x'*P*x
        Ak %: A + BK closed loop dynamics
    end

    methods (Access = public)

        function obj = LinearSystem(A, B, Q, R)
            obj.A = A;
            obj.B = B;
            obj.Q = Q;
            obj.R = R;
            obj.nx = size(A, 1);
            obj.nu = size(B, 2);

            [K_tmp, obj.P] = dlqr(obj.A, obj.B, obj.Q, obj.R);
            obj.K = -K_tmp;
            obj.Ak = (obj.A+obj.B*obj.K);
        end

        function x_new = propagate(obj, x, u) 
            x_new = obj.A * x + obj.B * u;
        end

        function Xmpi = compute_MPIset(obj, Xc, Uc) 
            [F, G, nc] = convert_Poly2Mat(Xc, Uc);
            Fpi = @(i) (F+G*obj.K)*obj.Ak^i;
            Xpi = @(i) Polyhedron(Fpi(i), ones(size(Fpi(i), 1), 1));
            Xmpi = Xpi(0);
            i= 0;
            while(1) % 
                i = i + 1;
                Xmpi_tmp = and(Xmpi, Xpi(i));
                if Xmpi_tmp == Xmpi
                    break;
                else
                    Xmpi = Xmpi_tmp;
                end
            end
        end
    end
end



================================================
FILE: src/ModelPredictiveControl.m
================================================
classdef ModelPredictiveControl < handle
    
    properties (SetAccess = private)
        sys % linear sys
        optcon; % optimal contol solver object
        Xc
        solution_cache
    end
    
    methods (Access = public)
        
        function obj = ModelPredictiveControl(sys, Xc, Uc, N)
            obj.sys = sys;
            obj.Xc = Xc;
            obj.optcon = OptimalControler(sys, Xc, Uc, N);
            obj.solution_cache = [];
        end

        function u_next = solve(obj, x_init)
            obj.optcon.add_initial_eq_constraint(x_init);
            [x_nominal_seq, u_nominal_seq] = obj.optcon.solve();
            obj.solution_cache = struct(...
                'x_init', x_init', 'x_nominal_seq', x_nominal_seq, 'u_nominal_seq', u_nominal_seq);
            u_next = u_nominal_seq(:, 1);
        end

        function show_prediction(obj)
            assert(~isempty(obj.solution_cache), 'can be used only after solved');
            Graphics.show_convex(obj.Xc, 'm');

            x_init = obj.solution_cache.x_init;
            scatter(x_init(1), x_init(2), 50, 'bs', 'filled');

            x_nominal_seq = obj.solution_cache.x_nominal_seq;
            Graphics.show_trajectory(x_nominal_seq, 'gs-');

            leg = legend({'$X_c$', 'current state', 'nominal traj.'}, 'position', [0.5 0.15 0.1 0.2]);
            set(leg, 'Interpreter', 'latex');
        end
    end
    
end


================================================
FILE: src/OptimalControler.m
================================================
classdef OptimalControler < handle
    % Let s :=[x(0), .., x(n), u(0)....u(n-1)] be decision variables, then
    % optimal control problem is composed by
    % (i) objective function V(s) = s'*H*S in quadratic form
    % (ii) equality constraints specified by C_eq1 and C_eq2 where C_eq1*s =C_eq2
    % (iii) inequality constraints specified by C_ineq1 and C_ineq2 where C_ineq1 * s <= C_ineq2
    %
    % For potential future extensions, all constraints will be managed by ConstraintManager class.
    % Any time you add a new constraint, those constraint is pushed into the manager with a key (name of constraint). 
    
    
    properties (SetAccess = private)
        sys; %system
        Xc; Uc; % constraints set for statespace and input space
        x_min; x_max; % lower and upper bound of Xc
        N; % prediction horizon
        Ak; % S.T.M of closed-roop system with LQR feedback
        n_opt; % dim. of optimization parameter s :=[x(0), .., x(n), u(0)....u(n-1)]
        H; % positive definite matrix for objective function V(s) = s'*H*s 
        constraint_manager;
    end
    
    %% Public Methods
    methods (Access = public)
        
        function obj = OptimalControler(sys, Xc, Uc, N)
            obj.sys = sys;
            obj.Xc = Xc;
            obj.x_min = min(Xc.V, [], 1)';
            obj.x_max = max(Xc.V, [], 2)';
            obj.Uc = Uc;
            obj.N = N;
            
            obj.n_opt = obj.sys.nx*(obj.N+1)+obj.sys.nu*obj.N;
            obj.constraint_manager = ConstraintManager();
            
            obj.H = obj.construct_costfunction();
            [C_eq1, C_eq2] = obj.construct_dynamics_constraint();
            [C_ineq1, C_ineq2] = obj.construct_ineq_constraint(Xc, Uc);

            %% Let's change initial and dynamics constraints!!
            obj.constraint_manager.add_eq_constraint('dynamics', C_eq1, C_eq2);
            obj.constraint_manager.add_ineq_constraint('feasible', C_ineq1, C_ineq2);
        end

        function add_initial_eq_constraint(obj, x_init)
            % E * x0 = x_init
            idx_x0_start = 1;
            idx_x0_end = obj.sys.nx;

            C_eq1_init = zeros(obj.sys.nx, obj.n_opt);
            C_eq1_init(:, idx_x0_start:idx_x0_end) = eye(obj.sys.nx);
            C_eq2_init = x_init;
            obj.constraint_manager.add_eq_constraint('initial', C_eq1_init, C_eq2_init);
        end

        function add_terminal_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, obj.N+1);
            obj.constraint_manager.add_ineq_constraint('terinal', C_ineq1_add, C_ineq2_add);
        end

        function add_initial_constraint(obj, Xadd)
            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, 1);
            obj.constraint_manager.add_ineq_constraint('initial', C_ineq1_add, C_ineq2_add);
        end
        
        function [x_seq, u_seq] = solve(obj)
            quadprog_solved = 0;
            [C_eq1, C_eq2] = obj.constraint_manager.combine_all_eq_constraints();
            [C_ineq1, C_ineq2] = obj.constraint_manager.combine_all_ineq_constraints();

            options = optimoptions('quadprog', 'Display', 'none');
            [var_optim, ~, exitflag] = quadprog(obj.H, [], C_ineq1, C_ineq2, C_eq1, C_eq2, [], [], [], options);
            x_seq = reshape(var_optim(1:obj.sys.nx*(obj.N+1)), obj.sys.nx, obj.N+1);
            u_seq = reshape(var_optim(obj.sys.nx*(obj.N+1)+1:obj.n_opt), obj.sys.nu, obj.N);
            
        end
        
    end
    
    %% Methods Used in Constoructor
    methods (Access = private)
        
        function H = construct_costfunction(obj)
            % compute H
            Q_block = [];
            R_block = [];
            for itr=1:obj.N
                Q_block = blkdiag(Q_block, obj.sys.Q);
                R_block = blkdiag(R_block, obj.sys.R);
            end
            H = blkdiag(Q_block, obj.sys.P, R_block);
        end

        function [C_eq1, C_eq2] = construct_dynamics_constraint(obj)
            % compute C_eq1 and C_eq2
            function C_ss_eq1 = single_step_dynamics_eq1(k)
                % A x(k) - E x(k+1) + Bu(k) = 0
                idx_xk_start = obj.sys.nx * k + 1;
                idx_xk_end = obj.sys.nx * (k + 1);

                idx_xkp1_start = obj.sys.nx * (k + 1) + 1;
                idx_xkp1_end = obj.sys.nx * (k + 2);

                idx_uk_start  = obj.sys.nx * (obj.N+1) + obj.sys.nu * k + 1;
                idx_uk_end  = obj.sys.nx * (obj.N+1) + obj.sys.nu * (k + 1);

                C_ss_eq1 = zeros(obj.sys.nx, obj.n_opt);

                C_ss_eq1(:, idx_xk_start:idx_xk_end) = obj.sys.A;
                C_ss_eq1(:, idx_xkp1_start:idx_xkp1_end) = - eye(obj.sys.nx);
                C_ss_eq1(:, idx_uk_start:idx_uk_end) = obj.sys.B;
            end

            C_eq1 = [];
            for k = 0:obj.N-1
                C_ss_eq1 = single_step_dynamics_eq1(k);
                C_eq1 = [C_eq1; C_ss_eq1];
            end
            C_eq2 = zeros(size(C_eq1, 1), 1);
        end
       
        function [C_ineq1, C_ineq2] = construct_ineq_constraint(obj, Xc, Uc)
            % compute C_ineq
            [F, G, nc] = convert_Poly2Mat(Xc, Uc);
            
            F_block = [];
            G_block = [];
            for itr = 1:obj.N
                G_block = blkdiag(G_block, G);
            end
            for itr = 1:obj.N+1
                F_block = blkdiag(F_block, F);
            end
            C_ineq1 = [F_block, [G_block; zeros(nc, obj.sys.nu*obj.N)]];
            nc_total = size(C_ineq1, 1);
            C_ineq2 = ones(nc_total, 1);
        end
        
    end
    
    methods (Access = private)
        
        function [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, k_add)
            % add a new constraint at time step k 
            if Xadd.contains(zeros(2, 1)) % If Xadd contains the origin, the contraint can be expressed as C1*x<=1
                [F_add, ~, nc_add] = convert_Poly2Mat(Xadd, Polyhedron());
                C_ineq2_add = ones(nc_add, 1);
                
            else % in other cases, expressed in a general affine form C1*x<=C2
                F_add = Xadd.A;
                nc_add = size(F_add, 1);
                C_ineq2_add = Xadd.b;
            end
            
            C_ineq1_add = zeros(nc_add, obj.n_opt);
            C_ineq1_add(:, (k_add-1)*obj.sys.nx+1:k_add*obj.sys.nx) = F_add;
        end
        
    end
end



================================================
FILE: src/TubeModelPredictiveControl.m
================================================
classdef TubeModelPredictiveControl < handle
    
    properties (SetAccess = public)
        sys % linear system with disturbance
        optcon; % optimal contol solver object
        Xc; % state constraint
        Uc; % input constraint
        Xc_robust; % Xc-Z (Pontryagin diff.)
        Xmpi_robust; % MPI set. Note that this is made using Xc-Z and Uc-KZ (instead of Xc and Uc)
        N; % horizon
        solution_cache
    end
    
    methods (Access = public)
        function obj = TubeModelPredictiveControl(sys, Xc, Uc, N)
            %----------approximation of d-inv set--------%
            %create robust X and U constraints, and construct solver using X and U
            Xc_robust = Xc - sys.Z;
            Uc_robust = Uc - sys.K*sys.Z;
            %optcon.reconstruct_ineq_constraint(Xc_robust, Uc_robust)
            optcon = OptimalControler(sys, Xc_robust, Uc_robust, N);

            %robustize Xmpi set and set it as a terminal constraint
            Xmpi_robust = sys.compute_MPIset(Xc_robust, Uc_robust);
            optcon.add_terminal_constraint(Xmpi_robust);

            %fill properteis
            obj.sys = sys;
            obj.optcon = optcon;
            obj.Xc = Xc;
            obj.Uc = Uc;
            obj.Xc_robust = Xc_robust;
            obj.Xmpi_robust = Xmpi_robust;
            obj.N = N;
            obj.solution_cache = [];
        end

        function u_next = solve(obj, x_init)
            % Note that solution of optimal controler is cached in obj.solution_cache
            Xinit = x_init + obj.sys.Z;
            obj.optcon.add_initial_constraint(Xinit);
            [x_nominal_seq, u_nominal_seq] = obj.optcon.solve();

            obj.solution_cache = struct(...
                'x_init', x_init', 'x_nominal_seq', x_nominal_seq, 'u_nominal_seq', u_nominal_seq);

            u_nominal = u_nominal_seq(:, 1);
            u_feedback = obj.sys.K * (x_init - x_nominal_seq(:, 1));
            u_next = u_nominal + u_feedback;
        end

        function [] = show_prediction(obj)
            assert(~isempty(obj.solution_cache), 'can be used only after solved');
            Graphics.show_convex(obj.Xc, 'm');
            Graphics.show_convex(obj.Xc_robust, 'r');
            Graphics.show_convex(obj.Xmpi_robust + obj.sys.Z, [0.3, 0.3, 0.3]); % rgb = [0.3, 0.3, 0.3]
            Graphics.show_convex(obj.Xmpi_robust, [0.5, 0.5, 0.5]); % gray
            x_init = obj.solution_cache.x_init;
            scatter(x_init(1), x_init(2), 50, 'bs', 'filled');
            x_nominal_seq = obj.solution_cache.x_nominal_seq;
            Graphics.show_trajectory(x_nominal_seq, 'gs-');
            for j=1:obj.N+1
                Graphics.show_convex(x_nominal_seq(:, j)+obj.sys.Z, 'g', 'FaceAlpha', 0.1);
            end

            leg = legend({'$X_c$', '$X_c\ominus Z$', '$X_f \oplus Z$', '$X_f (X_{mpi})$', 'current state', 'nominal traj.', 'tube'}, 'position', [0.5 0.15 0.1 0.2]);
            set(leg, 'Interpreter', 'latex');
        end
    end
end


================================================
FILE: src/utils/convert_Poly2Mat.m
================================================
function [F, G, nc] = convert_Poly2Mat(X, U)
    % Constraints set X and U in Polyhedron form -> F, G matrix form
    % nc; number of contraint forced by Xc and Uc
    % F; G; % constraints for state and input: Fx+Gu<=1, where 1 is a voctor
    poly2ineq = @(poly) poly.A./repmat(poly.b, 1, size(poly.A, 2));
    F_tmp = poly2ineq(X);
    G_tmp = poly2ineq(U);
    if numel(F_tmp)==0
        F_tmp = zeros(0, X.Dim);
    end
    if numel(G_tmp)==0
        G_tmp = zeros(0, U.Dim);
    end
    F = [F_tmp; zeros(size(G_tmp, 1), X.Dim)];
    G = [zeros(size(F_tmp, 1), U.Dim); G_tmp];
    nc = size(F, 1);
end


================================================
FILE: src/utils/number2string.m
================================================
function str = number2string(n)
    if n < 10
        str = strcat('0', string(n));
    else
        str = string(n);
    end
end