[
  {
    "path": ".gitignore",
    "content": "old/\n*.db\n*.asv\nexample/results/\n\npathdef.m\n/tbxmanager\n"
  },
  {
    "path": "LICENSE.md",
    "content": "MIT License\n\nCopyright (c) 2018 Hirokazu Ishida\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n"
  },
  {
    "path": "README.md",
    "content": "# Robust Model Predictive Control Using Tube\nThis repository includes examples for the tube model predictive control (tube-MPC)[1] as well as the generic model predictive control (MPC) written in MATLAB.\n\n## Requirement\n\n1) optimization_toolbox (matlab)<br>\n2) control_toolbox (matlab)<br>\n3) Multi-Parametric Toolbox 3 (open-source and freely available at http://people.ee.ethz.ch/~mpt/3/)\n\n## Feedback, bug reports, contributions\nIf 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.\n\n## Usage\nSee `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.\n\n## Short introduction to the tube MPC\nAfter running `example/example_tubeMPC.m`, you will get the following figure sequence.\n![the gif file](/fig/tube_mpc.gif)\n\nNow 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. \n\nLet 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`. \n\n## Disturbance invariant set\nI 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.\n\n## Maximum positively invariant set\nI 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.\n\n# Reference\n[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.\n[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.\n[3] Kouvaritakis, Basil, and Mark Cannon. \"Model predictive control.\" Switzerland: Springer International Publishing (2016).\n"
  },
  {
    "path": "example/example_MPC.m",
    "content": "addpath('../src/')\r\naddpath('../src/utils/')\r\n\r\n%the usage is mostly same as tubeMPC\r\nA = [1 1; 0 1];\r\nB = [0.5; 1]; \r\nQ = diag([1, 1]);\r\nR = 0.1;\r\nmysys = LinearSystem(A, B, Q, R);\r\n\r\nXc_vertex = [2, -2; 2 2; -10 2; -10 -2];\r\nUc_vertex = [1; -1];\r\nXc = Polyhedron(Xc_vertex);\r\nUc = Polyhedron(Uc_vertex);\r\n\r\n% Unlike tube-MPC, this generic MPC doesn't guarantee the robustness. \r\n% In the following simulation you will notice that the generated path almost touch the boundary.\r\n% Thus, you can imagine that if some additive disturbance is considered, then the state can \r\n% easily be off the boundary, and the succeeding optimization will no longer be feasible.\r\n% Please add some noise and do experiments.\r\nN_horizon = 5;\r\nmpc = ModelPredictiveControl(mysys, Xc, Uc, N_horizon);\r\n\r\nx = [-7; -2];\r\nsavedir_name = 'results';\r\nfor i = 1:15\r\n    u_next = mpc.solve(x);\r\n    x = mysys.propagate(x, u_next); % + add some noise here\r\n    mpc.show_prediction();\r\n    pause(0.1);\r\n    clf;\r\nend\r\n\r\n"
  },
  {
    "path": "example/example_dist_inv_set.m",
    "content": "addpath('../src/')\naddpath('../src/utils/')\n\n% specify your own discrete linear system\nA = [1 1; 0 1];\nB = [0.5; 1]; \nQ = diag([1, 1]);\nR = 0.1;\n\n% construct a convex set of system noise (2dim here)\nW_vertex = [0.15, 0.15; 0.15, -0.15; -0.15, -0.15; -0.15, 0.15];\nW = Polyhedron(W_vertex);\n\n% construct disturbance Linear system\n% note that disturbance invariant set Z is computed and stored as member variable in the constructor.\ndisturbance_system = DisturbanceLinearSystem(A, B, Q, R, W); \n\n% you can see that with any disturbance bounded by W, the state is guaranteed to inside Z\nx = zeros(2); % initial state \nfor i = 1:50\n    u = disturbance_system.K * (x - 0);\n    x = disturbance_system.propagate(x, u); % disturbance is considered inside the method\n    clf;\n    Graphics.show_convex(disturbance_system.Z, 'g', 'FaceAlpha', .3); % show Z\n    scatter(x(1, :), x(2, :)); % show particle\n    pause(0.01);\nend\n"
  },
  {
    "path": "example/example_optimalcontrol.m",
    "content": "addpath('../src/')\r\naddpath('../src/utils/')\r\n\r\n% make your own discrete linear system\r\nA = [1 1; 0 1];\r\nB = [0.5; 1]; \r\nQ = diag([1, 1]);\r\nR = 0.1;\r\nmysys = LinearSystem(A, B, Q, R); \r\n\r\n% constraints on 2dim state Xc and 1dim input Uc\r\nXc_vertex = [2, -2; 2 2; -10 2; -10 -2];\r\nUc_vertex = [1; -1];\r\nXc = Polyhedron(Xc_vertex);\r\nUc = Polyhedron(Uc_vertex);\r\n\r\n% create a optimal controler\r\n% if N_horizon is too small, the path will never reach inside \r\n% the maximum positively invariant set (X_MPI) in time step N_horizon, \r\n% then the problem becomes infeasible.\r\n% OptimalControler.solve method returns optimal path (x_seq) and input (u_seq)\r\nN_horizon = 20; \r\noptcon = OptimalControler(mysys, Xc, Uc, 20);\r\nx_init = [-7; -2];\r\noptcon.add_initial_eq_constraint(x_init);\r\n[x_seq, u_seq] = optcon.solve();\r\n"
  },
  {
    "path": "example/example_tubeMPC.m",
    "content": "addpath('../src/')\r\naddpath('../src/utils/')\r\n\r\n% fix random seed\r\nrng(0);\r\n\r\n% make your own discrete linear system with disturbance\r\nA = [1 1; 0 1];\r\nB = [0.5; 1]; \r\nQ = diag([1, 1]);\r\nR = 0.1;\r\n\r\nW_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)\r\nW = Polyhedron(W_vertex);\r\n\r\n% construct disturbance Linear system\r\ndisturbance_system = DisturbanceLinearSystem(A, B, Q, R, W);\r\n\r\n% constraints on state Xc and input Uc\r\nXc_vertex = [2, -2; 2 2; -10 2; -10 -2];\r\nUc_vertex = [1; -1];\r\nXc = Polyhedron(Xc_vertex);\r\nUc = Polyhedron(Uc_vertex);\r\n\r\n% create a tube_mpc simulater\r\n% 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. \r\nN_horizon = 10;\r\nw_min = [0; -0.10];\r\nw_max = [0; 0.10];\r\nmpc = TubeModelPredictiveControl(disturbance_system, Xc, Uc, N_horizon);\r\n\r\n% The robust MPC guidances the path inside the robust MPI-set so that the path will reach the robust MPI-set in N_horizon. \r\nx = [-7; -2];\r\nsavedir_name = './results/';\r\nmkdir(savedir_name);\r\n\r\nfor i = 1:15\r\n    disp(i)\r\n    u_next = mpc.solve(x);\r\n    x = disturbance_system.propagate(x, u_next); % additive disturbance is considered inside the method \r\n    mpc.show_prediction();\r\n    filename = strcat(savedir_name, 'tmpc_seq', number2string(i), '.png')\r\n    saveas(gcf, char(filename)); % removing this line makes the code much faster\r\n    clf;\r\nend\r\n\r\n"
  },
  {
    "path": "note/.gitignore",
    "content": "*.aux\n*.dvi\n*.log\n*.out\n\n"
  },
  {
    "path": "note/idea.tex",
    "content": "\\documentclass[15pt]{article}\n\\usepackage{cancel}\n\\usepackage{amsmath}\n\\usepackage{listings}\n\\usepackage{amsthm}\n\\usepackage{amssymb} %symbols for mapping\n\\usepackage{amsfonts}\n\\usepackage{mathptmx}\n\\usepackage{bm}\n\\usepackage{framed}\n\\usepackage{geometry}\n\\geometry{left=20mm,right=20mm,top=20mm,bottom=20mm}\n\\usepackage{hyperref}\n\\usepackage{mathtools}\n\\usepackage{color}\n\\usepackage{comment}\n\\usepackage{hyperref}\n\\newcommand{\\argmax}{\\mathop{\\rm argmax}\\limits}\n\\newcommand{\\argmin}{\\mathop{\\rm argmin}\\limits}\n\\newcommand{\\mymax}{\\mathop{\\rm max}\\limits}\n\\newcommand{\\mymin}{\\mathop{\\rm min}\\limits}\n\\newcommand{\\np}{\\mathcal{N}_p}\n\n\\definecolor{dkgreen}{rgb}{0,0.6,0}\n\\definecolor{gray}{rgb}{0.5,0.5,0.5}\n\\definecolor{mauve}{rgb}{0.58,0,0.82}\n\n\\lstset{frame=tb,\n  language=matlab,\n  aboveskip=3mm,\n  belowskip=3mm,\n  showstringspaces=false,\n  columns=flexible,\n  basicstyle={\\small\\ttfamily},\n  numbers=none,\n  numberstyle=\\tiny\\color{gray},\n  keywordstyle=\\color{blue},\n  commentstyle=\\color{dkgreen},\n  stringstyle=\\color{mauve},\n  breaklines=true,\n  breakatwhitespace=true,\n  tabsize=3\n}\n\n\\hypersetup{\n  urlcolor=cyan\n}\n\\begin{document}\n\\begin{center}\n  \\textbf{Possible extension to distributed MPC} \n\\end{center}\n\nLet 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, \n\\begin{align}\n  \\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) \\\\\n   & = (\\#\\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\n\\end{align}\nLetting $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 \n\\[\n  x_p^TAx_p + b x_p + c\n\\]\nWe 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 \n\\begin{align}\n  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}} \\\\\n  &= \\left( X^T \\tilde{A} X + \\tilde{b} X + \\tilde{c} \\right)  + U^T \\tilde{R} U \\\\\n  &= S^T \\mathrm{diag}(\\tilde{A}, \\tilde{R}) S + [\\tilde{b}, \\mathrm{zeros}]^T S + \\tilde{c}\\\\\n\\end{align} \nwher $\\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)$. \n\nIn my original implementation, the cost $J$ is expressed by $S^T H S$ and $H$ is constructed by the following:\n\\begin{lstlisting}\nfunction H = construct_costfunction(obj)\n    % compute H\n    Q_block = [];\n    R_block = [];\n    for itr=1:obj.N\n        Q_block = blkdiag(Q_block, obj.sys.Q);\n        R_block = blkdiag(R_block, obj.sys.R);\n    end\n    H = blkdiag(Q_block, obj.sys.P, R_block);\nend\n\\end{lstlisting}\nSo 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$).\n\\end{document}\n"
  },
  {
    "path": "src/ConstraintManager.m",
    "content": "classdef ConstraintManager < handle\n\n    properties (SetAccess = private)\n        C_equality;\n        C_inequality;\n    end\n\n    methods (Access = public)\n\n        function obj = ConstraintManager()\n            obj.C_equality = containers.Map();\n            obj.C_inequality = containers.Map();\n        end\n\n        function add_eq_constraint(obj, key, C_eq1, C_eq2)\n            obj.C_equality(key) =  {C_eq1, C_eq2};\n        end\n\n        function add_ineq_constraint(obj, key, C_ineq1, C_ineq2)\n            obj.C_inequality(key) =  {C_ineq1, C_ineq2};\n        end\n\n        function remove_eq_constraint(obj, key)\n            remove(obj.C_equality, key);\n        end\n\n        function remove_ineq_constraint(obj, key)\n            remove(obj.C_inequality, key);\n        end\n\n        function [C_eq1, C_eq2] = combine_all_eq_constraints(obj)\n            C_eq_array = values(obj.C_equality);\n            C_eq1 = [];\n            C_eq2 = [];\n            for i = 1:numel(C_eq_array)\n                C_eq1 = [C_eq1; C_eq_array{i}{1}];\n                C_eq2 = [C_eq2; C_eq_array{i}{2}];\n            end\n        end\n\n        function [C_ineq1, C_ineq2] = combine_all_ineq_constraints(obj)\n            C_ineq_array = values(obj.C_inequality);\n            C_ineq1 = [];\n            C_ineq2 = [];\n            for i = 1:numel(C_ineq_array)\n                C_ineq1 = [C_ineq1; C_ineq_array{i}{1}];\n                C_ineq2 = [C_ineq2; C_ineq_array{i}{2}];\n            end\n        end\n    end\nend\n\n"
  },
  {
    "path": "src/DisturbanceLinearSystem.m",
    "content": "classdef DisturbanceLinearSystem < LinearSystem\r\n\r\n    properties (SetAccess = private)\r\n        W % convex set of distrubance\r\n        Z % disturbance invariant set\r\n    end\r\n\r\n    methods (Access = public)\r\n\r\n        function obj = DisturbanceLinearSystem(A, B, Q, R, W)\r\n            obj = obj@LinearSystem(A, B, Q, R);\r\n\r\n            obj.W = W;\r\n            obj.Z = obj.compute_mrpi_set(1e-4);\r\n        end\r\n\r\n        function x_new = propagate(obj, x, u)\r\n            w = obj.pick_random_disturbance();\r\n            x_new = propagate@LinearSystem(obj, x, u) + w;\r\n        end\r\n\r\n    end\r\n\r\n    methods (Access = public)\r\n\r\n        function w = pick_random_disturbance(obj)\r\n            % pick disturbance form uniform distribution\r\n            verts = obj.W.V;\r\n            b_max = max(verts)';\r\n            b_min = min(verts)';\r\n\r\n            % generate random until it will be inside of W\r\n            while true\r\n                w = rand(obj.nx, 1) .* (b_max - b_min) + b_min; \r\n                if obj.W.contains(w)\r\n                    break\r\n                end\r\n            end\r\n        end\r\n\r\n        function Fs = compute_mrpi_set(obj, epsilon)\r\n            % Computes an invariant approximation of the minimal robust positively\r\n            % invariant set for \r\n            % x^{+} = Ax + w with w \\in W\r\n            % according to Algorithm 1 in 'Invariant approximations of\r\n            % the minimal robust positively invariant set' by Rakovic et al. \r\n            % Requires a matrix A, a Polytope W, and a tolerance 'epsilon'.  \r\n            [nx,~] = size(obj.Ak); \r\n            s = 0; \r\n            alpha = 1000;\r\n            Ms = 1000;\r\n            E = eye(nx);\r\n            it = 0;\r\n            while(alpha > epsilon/(epsilon + Ms))\r\n                s = s+1;\r\n                alpha = max(obj.W.support(obj.Ak^s*(obj.W.A)')./obj.W.b);\r\n                mss = zeros(2*nx,1);\r\n                for i = 1:s\r\n                    mss = mss+obj.W.support([obj.Ak^i, -obj.Ak^i]);\r\n                end\r\n                Ms = max(mss);\r\n                it = it+1;\r\n            end\r\n\r\n            Fs = obj.W;\r\n            for i =1:s-1\r\n                Fs = Fs+obj.Ak^i*obj.W;\r\n            end\r\n            Fs = (1/(1-alpha))*Fs;\r\n        end\r\n\r\n    end\r\nend\r\n"
  },
  {
    "path": "src/Graphics.m",
    "content": "classdef Graphics<handle\r\n    % This is a function collection rather than a class\r\n    % So, All methods are static.\r\n    % plot trajectory and show convex set which are projected from n-dim\r\n    \r\n    methods (Static)\r\n        function show_convex(P, varargin)\r\n            P_reduced = projectPolytope2Plane(P);\r\n            fill(P_reduced.V(:, 1), P_reduced.V(:, 2), varargin{:})\r\n            hold on;\r\n        end\r\n\r\n        function show_trajectory(x_seq, varargin)\r\n            plot(x_seq(1, :), x_seq(2, :), varargin{:})\r\n            hold on;\r\n        end\r\n\r\n    end\r\nend\r\n\r\nfunction P_projected = projectPolytope2Plane(P)\r\n    vert = P.V;\r\n    x_vert = round(vert(:, 1), 5);\r\n    y_vert = round(vert(:, 2), 5);\r\n    idx = convhull(x_vert, y_vert);\r\n    P_projected = Polyhedron([x_vert(idx), y_vert(idx)]);\r\nend\r\n"
  },
  {
    "path": "src/LinearSystem.m",
    "content": "classdef LinearSystem < handle\n    properties (SetAccess = private)\n        % common notation for linear system\n        A; B; % dynamics x[k+1] = A*x[k]+B*u[k]\n        nx; nu; % dim of state space and input space\n        Q; R; % quadratic stage cost for LQR\n        K; % LQR feedback coefficient vector: u=Kx\n        P; % optimal cost function of LQR is x'*P*x\n        Ak %: A + BK closed loop dynamics\n    end\n\n    methods (Access = public)\n\n        function obj = LinearSystem(A, B, Q, R)\n            obj.A = A;\n            obj.B = B;\n            obj.Q = Q;\n            obj.R = R;\n            obj.nx = size(A, 1);\n            obj.nu = size(B, 2);\n\n            [K_tmp, obj.P] = dlqr(obj.A, obj.B, obj.Q, obj.R);\n            obj.K = -K_tmp;\n            obj.Ak = (obj.A+obj.B*obj.K);\n        end\n\n        function x_new = propagate(obj, x, u) \n            x_new = obj.A * x + obj.B * u;\n        end\n\n        function Xmpi = compute_MPIset(obj, Xc, Uc) \n            [F, G, nc] = convert_Poly2Mat(Xc, Uc);\n            Fpi = @(i) (F+G*obj.K)*obj.Ak^i;\n            Xpi = @(i) Polyhedron(Fpi(i), ones(size(Fpi(i), 1), 1));\n            Xmpi = Xpi(0);\n            i= 0;\n            while(1) % \n                i = i + 1;\n                Xmpi_tmp = and(Xmpi, Xpi(i));\n                if Xmpi_tmp == Xmpi\n                    break;\n                else\n                    Xmpi = Xmpi_tmp;\n                end\n            end\n        end\n    end\nend\n\n"
  },
  {
    "path": "src/ModelPredictiveControl.m",
    "content": "classdef ModelPredictiveControl < handle\r\n    \r\n    properties (SetAccess = private)\r\n        sys % linear sys\r\n        optcon; % optimal contol solver object\r\n        Xc\r\n        solution_cache\r\n    end\r\n    \r\n    methods (Access = public)\r\n        \r\n        function obj = ModelPredictiveControl(sys, Xc, Uc, N)\r\n            obj.sys = sys;\r\n            obj.Xc = Xc;\r\n            obj.optcon = OptimalControler(sys, Xc, Uc, N);\r\n            obj.solution_cache = [];\r\n        end\r\n\r\n        function u_next = solve(obj, x_init)\r\n            obj.optcon.add_initial_eq_constraint(x_init);\r\n            [x_nominal_seq, u_nominal_seq] = obj.optcon.solve();\r\n            obj.solution_cache = struct(...\r\n                'x_init', x_init', 'x_nominal_seq', x_nominal_seq, 'u_nominal_seq', u_nominal_seq);\r\n            u_next = u_nominal_seq(:, 1);\r\n        end\r\n\r\n        function show_prediction(obj)\r\n            assert(~isempty(obj.solution_cache), 'can be used only after solved');\r\n            Graphics.show_convex(obj.Xc, 'm');\r\n\r\n            x_init = obj.solution_cache.x_init;\r\n            scatter(x_init(1), x_init(2), 50, 'bs', 'filled');\r\n\r\n            x_nominal_seq = obj.solution_cache.x_nominal_seq;\r\n            Graphics.show_trajectory(x_nominal_seq, 'gs-');\r\n\r\n            leg = legend({'$X_c$', 'current state', 'nominal traj.'}, 'position', [0.5 0.15 0.1 0.2]);\r\n            set(leg, 'Interpreter', 'latex');\r\n        end\r\n    end\r\n    \r\nend\r\n"
  },
  {
    "path": "src/OptimalControler.m",
    "content": "classdef OptimalControler < handle\r\n    % Let s :=[x(0), .., x(n), u(0)....u(n-1)] be decision variables, then\r\n    % optimal control problem is composed by\r\n    % (i) objective function V(s) = s'*H*S in quadratic form\r\n    % (ii) equality constraints specified by C_eq1 and C_eq2 where C_eq1*s =C_eq2\r\n    % (iii) inequality constraints specified by C_ineq1 and C_ineq2 where C_ineq1 * s <= C_ineq2\r\n    %\r\n    % For potential future extensions, all constraints will be managed by ConstraintManager class.\r\n    % Any time you add a new constraint, those constraint is pushed into the manager with a key (name of constraint). \r\n    \r\n    \r\n    properties (SetAccess = private)\r\n        sys; %system\r\n        Xc; Uc; % constraints set for statespace and input space\r\n        x_min; x_max; % lower and upper bound of Xc\r\n        N; % prediction horizon\r\n        Ak; % S.T.M of closed-roop system with LQR feedback\r\n        n_opt; % dim. of optimization parameter s :=[x(0), .., x(n), u(0)....u(n-1)]\r\n        H; % positive definite matrix for objective function V(s) = s'*H*s \r\n        constraint_manager;\r\n    end\r\n    \r\n    %% Public Methods\r\n    methods (Access = public)\r\n        \r\n        function obj = OptimalControler(sys, Xc, Uc, N)\r\n            obj.sys = sys;\r\n            obj.Xc = Xc;\r\n            obj.x_min = min(Xc.V, [], 1)';\r\n            obj.x_max = max(Xc.V, [], 2)';\r\n            obj.Uc = Uc;\r\n            obj.N = N;\r\n            \r\n            obj.n_opt = obj.sys.nx*(obj.N+1)+obj.sys.nu*obj.N;\r\n            obj.constraint_manager = ConstraintManager();\r\n            \r\n            obj.H = obj.construct_costfunction();\r\n            [C_eq1, C_eq2] = obj.construct_dynamics_constraint();\r\n            [C_ineq1, C_ineq2] = obj.construct_ineq_constraint(Xc, Uc);\r\n\r\n            %% Let's change initial and dynamics constraints!!\r\n            obj.constraint_manager.add_eq_constraint('dynamics', C_eq1, C_eq2);\r\n            obj.constraint_manager.add_ineq_constraint('feasible', C_ineq1, C_ineq2);\r\n        end\r\n\r\n        function add_initial_eq_constraint(obj, x_init)\r\n            % E * x0 = x_init\r\n            idx_x0_start = 1;\r\n            idx_x0_end = obj.sys.nx;\r\n\r\n            C_eq1_init = zeros(obj.sys.nx, obj.n_opt);\r\n            C_eq1_init(:, idx_x0_start:idx_x0_end) = eye(obj.sys.nx);\r\n            C_eq2_init = x_init;\r\n            obj.constraint_manager.add_eq_constraint('initial', C_eq1_init, C_eq2_init);\r\n        end\r\n\r\n        function add_terminal_constraint(obj, Xadd)\r\n            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, obj.N+1);\r\n            obj.constraint_manager.add_ineq_constraint('terinal', C_ineq1_add, C_ineq2_add);\r\n        end\r\n\r\n        function add_initial_constraint(obj, Xadd)\r\n            [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, 1);\r\n            obj.constraint_manager.add_ineq_constraint('initial', C_ineq1_add, C_ineq2_add);\r\n        end\r\n        \r\n        function [x_seq, u_seq] = solve(obj)\r\n            quadprog_solved = 0;\r\n            [C_eq1, C_eq2] = obj.constraint_manager.combine_all_eq_constraints();\r\n            [C_ineq1, C_ineq2] = obj.constraint_manager.combine_all_ineq_constraints();\r\n\r\n            options = optimoptions('quadprog', 'Display', 'none');\r\n            [var_optim, ~, exitflag] = quadprog(obj.H, [], C_ineq1, C_ineq2, C_eq1, C_eq2, [], [], [], options);\r\n            x_seq = reshape(var_optim(1:obj.sys.nx*(obj.N+1)), obj.sys.nx, obj.N+1);\r\n            u_seq = reshape(var_optim(obj.sys.nx*(obj.N+1)+1:obj.n_opt), obj.sys.nu, obj.N);\r\n            \r\n        end\r\n        \r\n    end\r\n    \r\n    %% Methods Used in Constoructor\r\n    methods (Access = private)\r\n        \r\n        function H = construct_costfunction(obj)\r\n            % compute H\r\n            Q_block = [];\r\n            R_block = [];\r\n            for itr=1:obj.N\r\n                Q_block = blkdiag(Q_block, obj.sys.Q);\r\n                R_block = blkdiag(R_block, obj.sys.R);\r\n            end\r\n            H = blkdiag(Q_block, obj.sys.P, R_block);\r\n        end\r\n\r\n        function [C_eq1, C_eq2] = construct_dynamics_constraint(obj)\r\n            % compute C_eq1 and C_eq2\r\n            function C_ss_eq1 = single_step_dynamics_eq1(k)\r\n                % A x(k) - E x(k+1) + Bu(k) = 0\r\n                idx_xk_start = obj.sys.nx * k + 1;\r\n                idx_xk_end = obj.sys.nx * (k + 1);\r\n\r\n                idx_xkp1_start = obj.sys.nx * (k + 1) + 1;\r\n                idx_xkp1_end = obj.sys.nx * (k + 2);\r\n\r\n                idx_uk_start  = obj.sys.nx * (obj.N+1) + obj.sys.nu * k + 1;\r\n                idx_uk_end  = obj.sys.nx * (obj.N+1) + obj.sys.nu * (k + 1);\r\n\r\n                C_ss_eq1 = zeros(obj.sys.nx, obj.n_opt);\r\n\r\n                C_ss_eq1(:, idx_xk_start:idx_xk_end) = obj.sys.A;\r\n                C_ss_eq1(:, idx_xkp1_start:idx_xkp1_end) = - eye(obj.sys.nx);\r\n                C_ss_eq1(:, idx_uk_start:idx_uk_end) = obj.sys.B;\r\n            end\r\n\r\n            C_eq1 = [];\r\n            for k = 0:obj.N-1\r\n                C_ss_eq1 = single_step_dynamics_eq1(k);\r\n                C_eq1 = [C_eq1; C_ss_eq1];\r\n            end\r\n            C_eq2 = zeros(size(C_eq1, 1), 1);\r\n        end\r\n       \r\n        function [C_ineq1, C_ineq2] = construct_ineq_constraint(obj, Xc, Uc)\r\n            % compute C_ineq\r\n            [F, G, nc] = convert_Poly2Mat(Xc, Uc);\r\n            \r\n            F_block = [];\r\n            G_block = [];\r\n            for itr = 1:obj.N\r\n                G_block = blkdiag(G_block, G);\r\n            end\r\n            for itr = 1:obj.N+1\r\n                F_block = blkdiag(F_block, F);\r\n            end\r\n            C_ineq1 = [F_block, [G_block; zeros(nc, obj.sys.nu*obj.N)]];\r\n            nc_total = size(C_ineq1, 1);\r\n            C_ineq2 = ones(nc_total, 1);\r\n        end\r\n        \r\n    end\r\n    \r\n    methods (Access = private)\r\n        \r\n        function [C_ineq1_add, C_ineq2_add] = add_ineq_constraint(obj, Xadd, k_add)\r\n            % add a new constraint at time step k \r\n            if Xadd.contains(zeros(2, 1)) % If Xadd contains the origin, the contraint can be expressed as C1*x<=1\r\n                [F_add, ~, nc_add] = convert_Poly2Mat(Xadd, Polyhedron());\r\n                C_ineq2_add = ones(nc_add, 1);\r\n                \r\n            else % in other cases, expressed in a general affine form C1*x<=C2\r\n                F_add = Xadd.A;\r\n                nc_add = size(F_add, 1);\r\n                C_ineq2_add = Xadd.b;\r\n            end\r\n            \r\n            C_ineq1_add = zeros(nc_add, obj.n_opt);\r\n            C_ineq1_add(:, (k_add-1)*obj.sys.nx+1:k_add*obj.sys.nx) = F_add;\r\n        end\r\n        \r\n    end\r\nend\r\n\r\n"
  },
  {
    "path": "src/TubeModelPredictiveControl.m",
    "content": "classdef TubeModelPredictiveControl < handle\r\n    \r\n    properties (SetAccess = public)\r\n        sys % linear system with disturbance\r\n        optcon; % optimal contol solver object\r\n        Xc; % state constraint\r\n        Uc; % input constraint\r\n        Xc_robust; % Xc-Z (Pontryagin diff.)\r\n        Xmpi_robust; % MPI set. Note that this is made using Xc-Z and Uc-KZ (instead of Xc and Uc)\r\n        N; % horizon\r\n        solution_cache\r\n    end\r\n    \r\n    methods (Access = public)\r\n        function obj = TubeModelPredictiveControl(sys, Xc, Uc, N)\r\n            %----------approximation of d-inv set--------%\r\n            %create robust X and U constraints, and construct solver using X and U\r\n            Xc_robust = Xc - sys.Z;\r\n            Uc_robust = Uc - sys.K*sys.Z;\r\n            %optcon.reconstruct_ineq_constraint(Xc_robust, Uc_robust)\r\n            optcon = OptimalControler(sys, Xc_robust, Uc_robust, N);\r\n\r\n            %robustize Xmpi set and set it as a terminal constraint\r\n            Xmpi_robust = sys.compute_MPIset(Xc_robust, Uc_robust);\r\n            optcon.add_terminal_constraint(Xmpi_robust);\r\n\r\n            %fill properteis\r\n            obj.sys = sys;\r\n            obj.optcon = optcon;\r\n            obj.Xc = Xc;\r\n            obj.Uc = Uc;\r\n            obj.Xc_robust = Xc_robust;\r\n            obj.Xmpi_robust = Xmpi_robust;\r\n            obj.N = N;\r\n            obj.solution_cache = [];\r\n        end\r\n\r\n        function u_next = solve(obj, x_init)\r\n            % Note that solution of optimal controler is cached in obj.solution_cache\r\n            Xinit = x_init + obj.sys.Z;\r\n            obj.optcon.add_initial_constraint(Xinit);\r\n            [x_nominal_seq, u_nominal_seq] = obj.optcon.solve();\r\n\r\n            obj.solution_cache = struct(...\r\n                'x_init', x_init', 'x_nominal_seq', x_nominal_seq, 'u_nominal_seq', u_nominal_seq);\r\n\r\n            u_nominal = u_nominal_seq(:, 1);\r\n            u_feedback = obj.sys.K * (x_init - x_nominal_seq(:, 1));\r\n            u_next = u_nominal + u_feedback;\r\n        end\r\n\r\n        function [] = show_prediction(obj)\r\n            assert(~isempty(obj.solution_cache), 'can be used only after solved');\r\n            Graphics.show_convex(obj.Xc, 'm');\r\n            Graphics.show_convex(obj.Xc_robust, 'r');\r\n            Graphics.show_convex(obj.Xmpi_robust + obj.sys.Z, [0.3, 0.3, 0.3]); % rgb = [0.3, 0.3, 0.3]\r\n            Graphics.show_convex(obj.Xmpi_robust, [0.5, 0.5, 0.5]); % gray\r\n            x_init = obj.solution_cache.x_init;\r\n            scatter(x_init(1), x_init(2), 50, 'bs', 'filled');\r\n            x_nominal_seq = obj.solution_cache.x_nominal_seq;\r\n            Graphics.show_trajectory(x_nominal_seq, 'gs-');\r\n            for j=1:obj.N+1\r\n                Graphics.show_convex(x_nominal_seq(:, j)+obj.sys.Z, 'g', 'FaceAlpha', 0.1);\r\n            end\r\n\r\n            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]);\r\n            set(leg, 'Interpreter', 'latex');\r\n        end\r\n    end\r\nend\r\n"
  },
  {
    "path": "src/utils/convert_Poly2Mat.m",
    "content": "function [F, G, nc] = convert_Poly2Mat(X, U)\n    % Constraints set X and U in Polyhedron form -> F, G matrix form\n    % nc; number of contraint forced by Xc and Uc\n    % F; G; % constraints for state and input: Fx+Gu<=1, where 1 is a voctor\n    poly2ineq = @(poly) poly.A./repmat(poly.b, 1, size(poly.A, 2));\n    F_tmp = poly2ineq(X);\n    G_tmp = poly2ineq(U);\n    if numel(F_tmp)==0\n        F_tmp = zeros(0, X.Dim);\n    end\n    if numel(G_tmp)==0\n        G_tmp = zeros(0, U.Dim);\n    end\n    F = [F_tmp; zeros(size(G_tmp, 1), X.Dim)];\n    G = [zeros(size(F_tmp, 1), U.Dim); G_tmp];\n    nc = size(F, 1);\nend\n"
  },
  {
    "path": "src/utils/number2string.m",
    "content": "function str = number2string(n)\n    if n < 10\n        str = strcat('0', string(n));\n    else\n        str = string(n);\n    end\nend\n"
  }
]