Initial commit

.gitignore

@@ -0,0 +1,163 @@
+# MATLAB
+# Windows default autosave extension
+*.asv
+
+# OSX / *nix default autosave extension
+*.m~
+
+# Compiled MEX binaries (all platforms)
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Simulink code generation folders
+slprj/
+sccprj/
+
+# Matlab code generation folders
+codegen/
+
+# Simulink autosave extension
+*.autosave
+
+# Simulink cache files
+*.slxc
+
+# Octave session info
+octave-workspace
+
+# PYTHON
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+pip-wheel-metadata/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+.python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 Jun Zeng
+
+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.

README.md

@@ -0,0 +1,48 @@
+# MPC-CBF
+Model Predictive Control with Control Barrier Functions
+
+![](car-racing/demo.gif)
+
+# Prerequisites
+## Matlab
+The packages needed for running the code are [Yalmip](https://yalmip.github.io/) and [IPOPT](https://projects.coin-or.org/Ipopt/wiki/MatlabInterface).
+
+We also provide the zipped version of precompiled .mex files for IPOPT in the folder `packages` in case you don't have it. Unzip that file and add those .mex files into your MATLAB path.
+
+# Description
+## 2D double integrator
+The 2D double integrator is assigned to reach the target position at origin while avoiding obstacles. We have three classes for different controllers: `DCLF_DCBF.m` (DCLF-DCBF), `MPC_CBF.m` (MPC-CBF) and `MPC_DC` (MPC-DC), respectively.
+
+Moreover, to illustrate the performance among them, we have:
+* `main.m`: Run DCLF-DCBF/MPC-CBF/MPC-DC respectively.
+* `analysis_gamma.m`: Run analysis for different hyperparameter $\gamma$.
+* `analysis_horizon.m`: Run analysis for different horizon.
+
+We illustrate the performance between DCLF-DCBF/MPC-DC/MPC-CBF
+| DCLF-DCBF  | MPC-DC (N=8) |
+| --- | --- |
+| <img src="double-integrator-2D/figures/dclf-dcbf-avoidance.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-dc-avoidance.png" width="200" height="200"> |
+
+| MPC-CBF (N=1) | MPC-CBF (N=8) |
+| --- | --- |
+| <img src="double-integrator-2D/figures/mpc-cbf-avoidance-one-step.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.png" width="200" height="200"> |
+
+and also the safety performance for different numbers of horizon and hyperparameters
+| Different hyperparameter | Different horizon |
+| --- | --- |
+| <img src="double-integrator-2D/figures/benchmark-gamma.png" width="200" height="200"> | <img src="double-integrator-2D/figures/benchmark-horizon.png" width="200" height="200"> |
+
+## Car racing competition
+We have the speed profile and control input shown as follow,  
+
+<img src="car-racing/lmpc-speed-norm-profile.png" width="400">
+
+<img src="car-racing/lmpc-deviation-profile.png" width="400">
+
+<img src="car-racing/lmpc-input-profile.png" width="400">
+
+The animation can be found on the top of this readme, we will release full code after the paper is accepted. 
+
+# References
+This repository is based on the following:
+* Jun Zeng, Bike Zhang and Koushil Sreenath. "Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function." [[PDF]](http://arxiv.org/abs/2007.11718)

double-integrator-2D/DCLF_DCBF.m

@@ -0,0 +1,105 @@
+classdef DCLF_DCBF < handle
+    properties
+        system
+        params
+        x0
+        x_curr
+        time_curr = 0.0
+        xlog = []
+        ulog = []
+        u_cost = 0
+        obs
+    end
+    
+    methods
+        function self = DCLF_DCBF(x0, system, params)
+            % Define DCLF-DCBF controller
+            self.x0 = x0;
+            self.x_curr = x0;
+            self.system = system;
+            self.params = params;
+        end
+        
+        function sim(self, time)
+            % Simulate the system until a given time
+            xk = self.x_curr;
+            while self.time_curr <= time
+                % Solve DCLF-DCBF
+                uk = self.solve_dclf_dcbf();
+                xk = self.system.A * xk + self.system.B * uk;
+                % update system
+                self.x_curr = xk;
+                self.time_curr = self.time_curr + self.system.dt;
+                self.xlog = [self.xlog, xk];
+                self.ulog = [self.ulog, uk];
+                self.u_cost = self.u_cost + uk'*uk*self.system.dt;
+            end
+        end
+        
+        function uopt = solve_dclf_dcbf(self)
+            % Solve DCLF-DCBF
+            x = self.x_curr;
+            u = sdpvar(2,1);
+            delta = sdpvar(1,1);
+            constraints = [];
+            % add DCLF constraints
+            x_next = self.system.A * x + self.system.B * u;
+            v = x' * self.params.P * x;
+            v_next = x_next' * self.params.P * x_next;
+            constraints = [constraints; v_next - v + self.params.alpha * v <= delta];
+            % add DCBF constraints
+            pos = self.obs.pos;
+            r = self.obs.r;
+            b = (x(1:2)-pos)'*(x(1:2)-pos) - r^2;
+            b_next = (x_next(1:2)-pos)'*(x_next(1:2)-pos) - r^2;
+            constraints = [constraints; b_next - b + self.params.gamma*b >= 0];
+            % input constraints
+            constraints = [constraints; self.system.ul <= u <= self.system.uu];
+            % cost
+            cost = u'*self.params.H*u + delta'*self.params.p*delta;
+            % solve optimization
+            ops = sdpsettings('solver','ipopt','verbose',0);
+            diagnostics = optimize(constraints, cost, ops);
+            if diagnostics.problem == 0
+                feas = true;
+                uopt = value(u);
+            else
+                feas = false;
+                uopt = [];
+            end
+            fprintf('solver time: %f\n', diagnostics.solvertime);
+        end
+        
+        function plot(self,figure_name)
+            % Plot simulation
+            figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+            set(gca,'LooseInset',get(gca,'TightInset'));
+            hold on;
+            % plot trajectory
+            plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
+                'LineWidth', 1.0,'MarkerSize',4);
+            % plot obstacle
+            pos = self.obs.pos;
+            r = self.obs.r;
+            th = linspace(0,2*pi*100);
+            x = cos(th) ; y = sin(th) ;
+            plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
+            plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
+            % plot target position
+            plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
+            plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+            h=get(gca,'Children');
+            h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
+            set(h_legend, 'Interpreter','latex');
+            set(gca,'LineWidth', 0.2, 'FontSize', 15);
+            grid on
+            xlabel('$x (m)$','interpreter','latex','FontSize',20);
+            ylabel('$y (m)$','interpreter','latex','FontSize',20);
+            xticks(-5:0);
+            yticks(-5:0);
+            xlim([-5,0.2]);
+            ylim([-5,0.2]);
+            print(gcf,strcat('figures/',figure_name), '-depsc');
+        end
+    end
+end

double-integrator-2D/MPC_CBF.m

@@ -0,0 +1,145 @@
+classdef MPC_CBF < handle
+    % MPC with distance constraints
+    properties
+        system
+        params
+        x0
+        x_curr
+        time_curr = 0.0
+        xlog = []
+        ulog = []
+        distlog = []
+        solvertime = []
+        xopenloop = {}
+        uopenloop = {}
+        u_cost = 0
+        obs
+    end
+    methods
+        function self = MPC_CBF(x0, system, params)
+            % Define MPC_CBF controller
+            self.x0 = x0;
+            self.x_curr = x0;
+            self.system = system;
+            self.params = params;
+        end
+        
+        function sim(self, time)
+            % Simulate the system until a given time
+            xk = self.x_curr;
+            while self.time_curr <= time
+                % Solve CFTOC
+                [~, uk] = self.solve_mpc_dc(self.x_curr);
+                xk = self.system.A * xk + self.system.B * uk;
+                % update system
+                self.x_curr = xk;
+                self.time_curr = self.time_curr + self.system.dt;
+                self.xlog = [self.xlog, xk];
+                self.ulog = [self.ulog, uk];
+                self.u_cost = self.u_cost + uk'*uk*self.system.dt;
+            end
+        end
+        
+        function [xopt, uopt] = solve_mpc_dc(self, xk)
+            % Solve MPC-CBF
+            [feas, x, u, J] = self.solve_cftoc(xk);
+            if ~feas
+                xopt = [];
+                uopt = [];
+                return
+            else
+                xopt = x(:,2);
+                uopt = u(:,1);
+            end
+        end
+        
+        function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
+            % Solve CFTOC
+            % extract variables
+            N = self.params.N;
+            % define variables and cost
+            x = sdpvar(4, N+1);
+            u = sdpvar(2, N);
+            constraints = [];
+            cost = 0;
+            % initial constraint
+            constraints = [constraints; x(:,1) == xk];
+            % add constraints and costs
+            for i = 1:N
+                constraints = [constraints;
+                    self.system.xl <= x(:,i) <= self.system.xu;
+                    self.system.ul <= u(:,i) <= self.system.uu
+                    x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
+                cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
+            end
+            % add CBF constraints
+            for i = 1:N
+                pos = self.obs.pos;
+                r = self.obs.r ;
+                b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
+                b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos)) - r^2;
+                constraints = [constraints; b_next - b + self.params.gamma * b >= 0];
+            end
+            % add terminal cost
+            cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
+            ops = sdpsettings('solver','ipopt','verbose',0);
+            % solve optimization
+            diagnostics = optimize(constraints, cost, ops);
+            if diagnostics.problem == 0
+                feas = true;
+                xopt = value(x);
+                uopt = value(u);
+                Jopt = value(cost);
+            else
+                feas = false;
+                xopt = [];
+                uopt = [];
+                Jopt = [];
+            end
+            self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
+            self.xopenloop{size(self.xopenloop,2)+1} = xopt;
+            self.uopenloop{size(self.uopenloop,2)+1} = uopt;
+            self.solvertime = [self.solvertime, diagnostics.solvertime];
+            fprintf('solver time: %f\n', diagnostics.solvertime);
+        end
+        
+        function plot(self, figure_name)
+            % Plot simulation
+            figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+            set(gca,'LooseInset',get(gca,'TightInset'));
+            hold on;
+            % plot closed-loop trajectory
+            plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
+                'LineWidth', 1.0, 'MarkerSize', 4);
+            % plot open-loop trajectory
+            for i = 1:size(self.xopenloop, 2)
+                plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
+                    'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
+            end
+            % plot obstacle
+            pos = self.obs.pos;
+            r = self.obs.r;
+            th = linspace(0,2*pi*100);
+            x = cos(th) ; y = sin(th) ;
+            plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
+                'LineWidth', 2);
+            plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
+                'MarkerSize', 5, 'LineWidth', 2);
+            % plot target position
+            plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
+            plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+            h=get(gca,'Children');
+            h_legend = legend(h([end]), {'MPC-CBF'}, 'Location', 'SouthEast');
+            set(h_legend, 'Interpreter','latex');
+            set(gca,'LineWidth', 0.2, 'FontSize', 15);
+            grid on
+            xlabel('$x (m)$','interpreter','latex','FontSize',20);
+            ylabel('$y (m)$','interpreter','latex','FontSize',20);
+            xticks(-5:0);
+            yticks(-5:0);
+            xlim([-5,0.2]);
+            ylim([-5,0.2]);
+            print(gcf,strcat('figures/',figure_name), '-depsc');
+        end
+    end
+end

double-integrator-2D/MPC_DC.m

@@ -0,0 +1,144 @@
+classdef MPC_DC < handle
+    % MPC with distance constraints
+    properties
+        system
+        params
+        x0
+        x_curr
+        time_curr = 0.0
+        xlog = []
+        ulog = []
+        distlog = []
+        solvertime = []
+        xopenloop = {}
+        uopenloop = {}
+        u_cost = 0
+        obs
+    end
+    methods
+        function self = MPC_DC(x0, system, params)
+            % Define MPC_DC controller
+            self.x0 = x0;
+            self.x_curr = x0;
+            self.system = system;
+            self.params = params;
+        end
+        
+        function sim(self, time)
+            % Simulate the system until a given time
+            xk = self.x_curr;
+            while self.time_curr <= time
+                % Solve CFTOC
+                [~, uk] = self.solve_mpc_dc(self.x_curr);
+                xk = self.system.A * xk + self.system.B * uk;
+                % update system
+                self.x_curr = xk;
+                self.time_curr = self.time_curr + self.system.dt;
+                self.xlog = [self.xlog, xk];
+                self.ulog = [self.ulog, uk];
+                self.u_cost = self.u_cost + uk'*uk*self.system.dt;
+            end
+        end
+        
+        function [xopt, uopt] = solve_mpc_dc(self, xk)
+            % Solve MPC-DC
+            [feas, x, u, J] = self.solve_cftoc(xk);
+            if ~feas
+                xopt = [];
+                uopt = [];
+                return
+            else
+                xopt = x(:,2);
+                uopt = u(:,1);
+            end
+        end
+        
+        function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
+            % Solve CFTOC
+            % extract variables
+            N = self.params.N;
+            % define variables and cost
+            x = sdpvar(4, N+1);
+            u = sdpvar(2, N);
+            constraints = [];
+            cost = 0;
+            % initial constraint
+            constraints = [constraints; x(:,1) == xk];
+            % add constraints and costs
+            for i = 1:N
+                constraints = [constraints;
+                    self.system.xl <= x(:,i) <= self.system.xu;
+                    self.system.ul <= u(:,i) <= self.system.uu
+                    x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
+                cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
+            end
+            % add CBF constraints
+            for i = 1:N
+                pos = self.obs.pos;
+                r = self.obs.r ;
+                b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
+                constraints = [constraints; b >= 0];
+            end
+            % add terminal cost
+            cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
+            ops = sdpsettings('solver','ipopt','verbose',0);
+            % solve optimization
+            diagnostics = optimize(constraints, cost, ops);
+            if diagnostics.problem == 0
+                feas = true;
+                xopt = value(x);
+                uopt = value(u);
+                Jopt = value(cost);
+            else
+                feas = false;
+                xopt = [];
+                uopt = [];
+                Jopt = [];
+            end
+            self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
+            self.xopenloop{size(self.xopenloop,2)+1} = xopt;
+            self.uopenloop{size(self.uopenloop,2)+1} = uopt;
+            self.solvertime = [self.solvertime, diagnostics.solvertime];
+            fprintf('solver time: %f\n', diagnostics.solvertime);
+        end
+        
+        function plot(self, figure_name)
+            % Plot simulation
+            figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+            set(gca,'LooseInset',get(gca,'TightInset'));
+            hold on;
+            % plot closed-loop trajectory
+            plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
+                'LineWidth', 1.0, 'MarkerSize', 4);
+            % plot open-loop trajectory
+            for i = 1:size(self.xopenloop, 2)
+                plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
+                    'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
+            end
+            % plot obstacle
+            pos = self.obs.pos;
+            r = self.obs.r;
+            th = linspace(0,2*pi*100);
+            x = cos(th) ; y = sin(th) ;
+            plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
+                'LineWidth', 2);
+            plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
+                'MarkerSize', 5, 'LineWidth', 2);
+            % plot target position
+            plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
+            plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+            h=get(gca,'Children');
+            h_legend = legend(h([end]), {'MPC-DC'}, 'Location', 'SouthEast');
+            set(h_legend, 'Interpreter','latex');
+            set(gca,'LineWidth', 0.2, 'FontSize', 15);
+            grid on
+            xlabel('$x (m)$','interpreter','latex','FontSize',20);
+            ylabel('$y (m)$','interpreter','latex','FontSize',20);
+            xticks(-5:0);
+            yticks(-5:0);
+            xlim([-5,0.2]);
+            ylim([-5,0.2]);
+            print(gcf,strcat('figures/',figure_name), '-depsc');
+        end
+    end
+end

double-integrator-2D/analysis_benchmark.m

@@ -0,0 +1,66 @@
+close all
+clear
+
+%% Setup and Parameters
+x0 = [-5; -5; 0; 0];
+time_total = 20.0;
+dt = 0.2;
+P = 100*eye(4);
+Q = 10*eye(4);
+R = eye(2);
+N = 8;
+umin = [-1; -1];
+umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
+
+%% Discrete-time double integrator 2D
+system.dt = dt;
+system.A = [1 0 dt 0;
+    0 1 0 dt;
+    0 0 1 0;
+    0 0 0 1];
+system.B = [0.5*dt^2 0;
+    0 0.5*dt^2;
+    dt 0;
+    0 dt];
+system.xl = xmin;
+system.xu = xmax;
+system.ul = umin;
+system.uu = umax;
+
+%% MPC-CBF parameters
+params.Q = Q;
+params.R = R;
+params.P = P;
+params.N = N;
+params.gamma = 0.5;
+
+%% Obstacle
+obs.pos = [-2; -2.25];
+obs.r = 1.5;
+
+%% Simulate MPC-CBF
+gamma_list = linspace(0.1, 0.5, 5);
+controller_mpc_cbf_list = {};
+for i = 1:size(gamma_list, 2)
+    new_params = params;
+    new_params.N = 5;
+    new_params.gamma = gamma_list(i);
+    controller_mpc_cbf = MPC_CBF(x0, system, new_params);
+    controller_mpc_cbf.obs = obs;
+    controller_mpc_cbf.sim(time_total);
+    controller_mpc_cbf_list{i} = controller_mpc_cbf;
+end
+
+%% Computational time benchmark
+for i = 1:size(gamma_list, 2)
+    controller_mpc_cbf = controller_mpc_cbf_list{i};
+    fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_cbf.solvertime),...
+    std(controller_mpc_cbf.solvertime),...
+    min(controller_mpc_cbf.solvertime),...
+    max(controller_mpc_cbf.solvertime),...
+    controller_mpc_cbf.u_cost,...
+    min(controller_mpc_cbf.distlog)]);
+end

double-integrator-2D/analysis_gamma.m

@@ -0,0 +1,95 @@
+close all
+clear
+
+%% Setup and Parameters
+x0 = [-5; -5; 0; 0];
+time_total = 20.0;
+dt = 0.2;
+P = 100*eye(4);
+Q = 10*eye(4);
+R = eye(2);
+N = 8;
+umin = [-1; -1];
+umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
+
+%% Discrete-time double integrator 2D
+system.dt = dt;
+system.A = [1 0 dt 0;
+    0 1 0 dt;
+    0 0 1 0;
+    0 0 0 1];
+system.B = [0.5*dt^2 0;
+    0 0.5*dt^2;
+    dt 0;
+    0 dt];
+system.xl = xmin;
+system.xu = xmax;
+system.ul = umin;
+system.uu = umax;
+
+%% MPC-CBF parameters
+params_mpc_cbf.Q = Q;
+params_mpc_cbf.R = R;
+params_mpc_cbf.P = P;
+params_mpc_cbf.N = N;
+params_mpc_cbf.gamma = 0.5;
+
+%% Obstacle
+obs.pos = [-2; -2.25];
+obs.r = 1.5;
+
+%% Simulate MPC-CBF
+gamma_list = [0.1, 0.2, 0.3, 1.0]; 
+for ind = 1:size(gamma_list, 2)
+    fprintf('Run MPC-CBF with gamma %f\n', gamma_list(ind));
+    params_mpc_cbf.gamma = gamma_list(ind);
+    controller_mpc_cbf_list{ind} = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_list{ind}.obs = obs;
+    controller_mpc_cbf_list{ind}.sim(time_total);
+end
+
+%% Simulate MPC-DC
+params_mpc_dc = params_mpc_cbf;
+controller_mpc_dc = MPC_DC(x0, system, params_mpc_cbf);
+controller_mpc_dc.obs = obs;
+controller_mpc_dc.sim(time_total);
+
+%% Visualization benchmark
+figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+set(gca,'LooseInset',get(gca,'TightInset'));
+hold on
+for ind = 1:size(gamma_list, 2)
+    plot(controller_mpc_cbf_list{ind}.xlog(1,:), controller_mpc_cbf_list{ind}.xlog(2,:),...
+        '-', 'LineWidth', 1.0, 'MarkerSize', 4);
+end
+plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:),...
+    'k--', 'LineWidth', 1.0, 'MarkerSize', 4);
+% plot obstacle
+pos = obs.pos;
+r = obs.r;
+th = linspace(0,2*pi*100);
+x = cos(th) ; y = sin(th) ;
+plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
+    'LineWidth', 2);
+plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
+    'MarkerSize', 5, 'LineWidth', 2);
+% plot target position
+plot(x0(1), x0(2), 'db', 'LineWidth', 1);
+plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+h=get(gca,'Children');
+h_legend = legend(h([end, end-1, end-2, end-3, end-4]),...
+    {'MPC-CBF ($\gamma=0.1$)', 'MPC-CBF ($\gamma=0.2$)',...
+    'MPC-CBF ($\gamma=0.3$)', 'MPC-CBF ($\gamma=1.0$)', 'MPC-DC'}, 'Location', 'SouthEast');
+set(h_legend, 'Interpreter','latex');
+set(gca,'LineWidth', 0.2, 'FontSize', 15);
+grid on
+xlabel('$x (m)$','interpreter','latex','FontSize',20);
+ylabel('$y (m)$','interpreter','latex','FontSize',20);
+xticks(-5:1);
+yticks(-5:1);
+xlim([-5,0.2]);
+ylim([-5,0.2]);
+print(gcf,'figures/benchmark-gamma.eps', '-depsc');
+print(gcf,'figures/benchmark-gamma.png', '-dpng', '-r800');

double-integrator-2D/analysis_horizon.m

@@ -0,0 +1,198 @@
+close all
+clear
+
+%% Setup and Parameters
+x0 = [-5; -5; 0; 0];
+time_total = 20.0;
+dt = 0.2;
+P = 100*eye(4);
+Q = 10*eye(4);
+R = eye(2);
+N = 8;
+umin = [-1; -1];
+umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
+
+%% Discrete-time double integrator 2D
+system.dt = dt;
+system.A = [1 0 dt 0;
+    0 1 0 dt;
+    0 0 1 0;
+    0 0 0 1];
+system.B = [0.5*dt^2 0;
+    0 0.5*dt^2;
+    dt 0;
+    0 dt];
+system.xl = xmin;
+system.xu = xmax;
+system.ul = umin;
+system.uu = umax;
+
+%% MPC-CBF parameters
+params.Q = Q;
+params.R = R;
+params.P = P;
+params.N = N;
+params.gamma = 0.5;
+
+%% Obstacle
+obs.pos = [-2; -2.25];
+obs.r = 1.5;
+
+%% Simulate MPC-CBF
+params.N = 5;
+params.gamma = 0.25;
+controller_mpc_cbf_1 = MPC_CBF(x0, system, params);
+controller_mpc_cbf_1.obs = obs;
+controller_mpc_cbf_1.sim(time_total);
+
+%% Simulate MPC-CBF with lower gamma
+params.gamma = 0.20;
+controller_mpc_cbf_2 = MPC_CBF(x0, system, params);
+controller_mpc_cbf_2.obs = obs;
+controller_mpc_cbf_2.sim(time_total);
+
+params.gamma = 0.15;
+controller_mpc_cbf_3 = MPC_CBF(x0, system, params);
+controller_mpc_cbf_3.obs = obs;
+controller_mpc_cbf_3.sim(time_total);
+
+%% Simulate MPC-DC
+params.N = 7;
+controller_mpc_dc_1 = MPC_DC(x0, system, params);
+controller_mpc_dc_1.obs = obs;
+controller_mpc_dc_1.sim(time_total);
+
+%% Simulate MPC-DC with larger horizon
+params.N = 15;
+controller_mpc_dc_2 = MPC_DC(x0, system, params);
+controller_mpc_dc_2.obs = obs;
+controller_mpc_dc_2.sim(time_total);
+
+%%
+params.N = 30;
+controller_mpc_dc_3 = MPC_DC(x0, system, params);
+controller_mpc_dc_3.obs = obs;
+controller_mpc_dc_3.sim(time_total);
+
+%% Visualization benchmark
+figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+set(gca,'LooseInset',get(gca,'TightInset'));
+hold on
+plot(controller_mpc_cbf_3.xlog(1,:), controller_mpc_cbf_3.xlog(2,:),...
+    '-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1.0, 'MarkerSize', 4);
+plot(controller_mpc_cbf_2.xlog(1,:), controller_mpc_cbf_2.xlog(2,:),...
+    '-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1.0, 'MarkerSize', 4);
+plot(controller_mpc_cbf_1.xlog(1,:), controller_mpc_cbf_1.xlog(2,:),...
+    '-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1.0, 'MarkerSize', 4);
+plot(controller_mpc_dc_1.xlog(1,:), controller_mpc_dc_1.xlog(2,:),...
+    '--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1.0, 'MarkerSize', 4);
+plot(controller_mpc_dc_2.xlog(1,:), controller_mpc_dc_2.xlog(2,:),...
+    '--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1.0, 'MarkerSize', 4);
+plot(controller_mpc_dc_3.xlog(1,:), controller_mpc_dc_3.xlog(2,:),...
+    '--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1.0, 'MarkerSize', 4);
+% plot obstacle
+pos = obs.pos;
+r = obs.r;
+th = linspace(0,2*pi*100);
+x = cos(th) ; y = sin(th) ;
+plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
+    'LineWidth', 2);
+plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
+    'MarkerSize', 5, 'LineWidth', 2);
+% plot target position
+plot(x0(1), x0(2), 'db', 'LineWidth', 1);
+plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+h=get(gca,'Children');
+h_legend = legend(h([end, end-1, end-2, end-3, end-4, end-5]),...
+    {'MPC-CBF ($N = 5, \gamma = 0.15$)', 'MPC-CBF ($N = 5, \gamma = 0.20$)',...
+    'MPC-CBF ($N = 5, \gamma = 0.25$)', 'MPC-DC ($N = 7$)', 'MPC-DC ($N = 15$)',...
+    'MPC-DC ($N = 30$)'}, 'Location', 'SouthEast');
+set(h_legend, 'Interpreter','latex', 'FontSize', 10);
+set(gca,'LineWidth', 0.2, 'FontSize', 15);
+grid on
+xlabel('$x (m)$','interpreter','latex','FontSize',20);
+ylabel('$y (m)$','interpreter','latex','FontSize',20);
+xticks(-5:0);
+yticks(-5:0);
+xlim([-5,0.2]);
+ylim([-5,0.2]);
+print(gcf,'figures/benchmark-horizon.eps', '-depsc');
+print(gcf,'figures/benchmark-horizon.png', '-dpng', '-r800');
+
+%% Computational time benchmark
+figure('Renderer', 'painters', 'Position', [0 0 800 400]);
+xlabel('Computational time (s)', 'interpreter', 'latex', 'FontSize', 16);
+ylabel('Percentage', 'interpreter', 'latex', 'FontSize', 16);
+hold on
+% [N_mpc_3, edges_mpc_3] = histcounts(controller_mpc_cbf_3.solvertime, 10, 'Normalization', 'probability');
+% plot(edges_mpc_3(1:end-1), N_mpc_3, '-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1);
+% [N_mpc_2, edges_mpc_2] = histcounts(controller_mpc_cbf_2.solvertime, 10, 'Normalization', 'probability');
+% plot(edges_mpc_2(1:end-1), N_mpc_2, '-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1);
+[N_mpc_1, edges_mpc_1] = histcounts(controller_mpc_cbf_1.solvertime, 10, 'Normalization', 'probability');
+plot(edges_mpc_1(1:end-1), N_mpc_1, '-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1);
+[N_dc_1, edges_dc_1] = histcounts(controller_mpc_dc_1.solvertime, 10, 'Normalization', 'probability');
+plot(edges_dc_1(1:end-1), N_dc_1, '--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1);
+[N_dc_2, edges_dc_2] = histcounts(controller_mpc_dc_2.solvertime, 10, 'Normalization', 'probability');
+plot(edges_dc_2(1:end-1), N_dc_2, '--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1);
+[N_dc_3, edges_dc_3] = histcounts(controller_mpc_dc_3.solvertime, 10, 'Normalization', 'probability');
+plot(edges_dc_3(1:end-1), N_dc_3, '--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1);
+h=get(gca,'Children');
+h_legend = legend(h([end, end-1, end-2, end-3]),...
+    {'MPC-CBF ($N = 5, \gamma = 0.25$)',...
+    'MPC-DC ($N = 7$)',...
+    'MPC-DC ($N = 15$)',...
+    'MPC-DC ($N = 30$)'}, 'Location', 'NorthEast');
+set(h_legend, 'Interpreter','latex');
+set(gca,'LineWidth', 0.2, 'FontSize', 15);
+print(gcf,'figures/benchmark-horizon-computational-time.eps', '-depsc');
+print(gcf,'figures/benchmark-horizon-computational-time.png', '-dpng', '-r800');
+%% Computational time table
+fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_cbf_3.solvertime),...
+    std(controller_mpc_cbf_3.solvertime),...
+    min(controller_mpc_cbf_3.solvertime),...
+    max(controller_mpc_cbf_3.solvertime),...
+    controller_mpc_cbf_3.u_cost,...
+    min(controller_mpc_cbf_3.distlog)]);
+
+fprintf('Computational time for MPC-CBF2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_cbf_2.solvertime),...
+    std(controller_mpc_cbf_2.solvertime),...
+    min(controller_mpc_cbf_2.solvertime),...
+    max(controller_mpc_cbf_2.solvertime),...
+    controller_mpc_cbf_2.u_cost,...
+    min(controller_mpc_cbf_2.distlog)]);
+
+fprintf('Computational time for MPC-CBF1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_cbf_1.solvertime),...
+    std(controller_mpc_cbf_1.solvertime),...
+    min(controller_mpc_cbf_1.solvertime),...
+    max(controller_mpc_cbf_1.solvertime),...
+    controller_mpc_cbf_1.u_cost,...
+    min(controller_mpc_cbf_1.distlog)]);
+
+fprintf('Computational time for MPC-DC1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_dc_1.solvertime),...
+    std(controller_mpc_dc_1.solvertime),...
+    min(controller_mpc_dc_1.solvertime),...
+    max(controller_mpc_dc_1.solvertime),...
+    controller_mpc_dc_1.u_cost,...
+    min(controller_mpc_dc_1.distlog)]);
+
+fprintf('Computational time for MPC-DC2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_dc_2.solvertime),...
+    std(controller_mpc_dc_2.solvertime),...
+    min(controller_mpc_dc_2.solvertime),...
+    max(controller_mpc_dc_2.solvertime),...
+    controller_mpc_dc_2.u_cost,...
+    min(controller_mpc_dc_2.distlog)]);
+
+fprintf('Computational time for MPC-DC3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
+    [mean(controller_mpc_dc_3.solvertime),...
+    std(controller_mpc_dc_3.solvertime),...
+    min(controller_mpc_dc_3.solvertime),...
+    max(controller_mpc_dc_3.solvertime),...
+    controller_mpc_dc_3.u_cost,...
+    min(controller_mpc_dc_3.distlog)]);

double-integrator-2D/main.m

@@ -0,0 +1,235 @@
+close all
+clear
+
+%% General Flags
+run_dclf_dcbf = true;
+display_dclf_dcbf = true;
+run_mpc_cbf_one = true;
+display_mpc_cbf_one = true;
+run_mpc_cbf_multiple = true;
+run_mpc_cbf_multiple = true;
+run_mpc_dc = true;
+display_mpc_dc = true;
+
+%% Setup and Parameters
+x0 = [-5; -5; 0; 0];
+time_total = 30.0;
+dt = 0.2;
+P = 100*eye(4);
+Q = 10*eye(4);
+R = eye(2);
+N = 8;
+umin = [-1; -1];
+umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
+
+%% Discrete-time double integrator 2D
+system.dt = dt;
+system.A = [1 0 dt 0;
+    0 1 0 dt;
+    0 0 1 0;
+    0 0 0 1];
+system.B = [0.5*dt^2 0;
+    0 0.5*dt^2;
+    dt 0;
+    0 dt];
+system.xl = xmin;
+system.xu = xmax;
+system.ul = umin;
+system.uu = umax;
+
+%% DCLF-DCBF parameters
+p = 1e3;
+params_dclf_dcbf.P = P;
+params_dclf_dcbf.H = R;
+params_dclf_dcbf.p = p;
+params_dclf_dcbf.alpha = 1.0;
+params_dclf_dcbf.gamma = 0.4;
+
+%% MPC-CBF parameters
+params_mpc_cbf.Q = Q;
+params_mpc_cbf.R = R;
+params_mpc_cbf.P = P;
+params_mpc_cbf.N = N;
+params_mpc_cbf.gamma = 0.4;
+
+%% MPC-DC parameters
+params_mpc_dc.Q = Q;
+params_mpc_dc.R = R;
+params_mpc_dc.P = P;
+params_mpc_dc.N = N;
+
+%% Obstacle
+obs.pos = [-2; -2.25];
+obs.r = 1.5;
+
+%% Simulate DCLF-DCBF
+if run_dclf_dcbf
+    fprintf('Run DCLF-DCBF\n');
+    controller_dclf_dcbf = DCLF_DCBF(x0, system, params_dclf_dcbf);
+    controller_dclf_dcbf.obs = obs;
+    controller_dclf_dcbf.sim(time_total);
+end
+
+%% Display DCLF-DCBF simulation
+if display_dclf_dcbf
+    % Plot simulation
+    figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+    set(gca,'LooseInset',get(gca,'TightInset'));
+    hold on;
+    % plot trajectory
+    plot(controller_dclf_dcbf.xlog(1,:), controller_dclf_dcbf.xlog(2,:), 'ko-',...
+        'LineWidth', 1.0,'MarkerSize',4);
+    % plot obstacle
+    pos = controller_dclf_dcbf.obs.pos;
+    r = controller_dclf_dcbf.obs.r;
+    th = linspace(0,2*pi*100);
+    x = cos(th) ; y = sin(th) ;
+    plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
+    plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
+    % plot target position
+    plot(controller_dclf_dcbf.x0(1), controller_dclf_dcbf.x0(2), 'db', 'LineWidth', 1);
+    plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+    h=get(gca,'Children');
+    h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
+    set(h_legend, 'Interpreter','latex');
+    set(gca,'LineWidth', 0.2, 'FontSize', 15);
+    grid on
+    xlabel('$x (m)$','interpreter','latex','FontSize',20);
+    ylabel('$y (m)$','interpreter','latex','FontSize',20);
+    xticks(-5:0);
+    yticks(-5:0);
+    xlim([-5,0.2]);
+    ylim([-5,0.2]);
+    print(gcf,'figures/dclf-dcbf-avoidance.eps', '-depsc');
+    print(gcf,'figures/dclf-dcbf-avoidance.png', '-dpng', '-r800');
+end
+
+%% Simulate MPC-CBF with N=1;
+params_mpc_cbf.N = 1;
+if run_mpc_cbf_one
+    fprintf('Run MPC-CBF\n');
+    controller_mpc_cbf_one = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_one.obs = obs;
+    controller_mpc_cbf_one.sim(time_total);
+end
+
+%% Display MPC-CBF simulation with N=1
+if display_mpc_cbf_one
+    % Plot simulation
+    figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+    set(gca,'LooseInset',get(gca,'TightInset'));
+    hold on;
+    % plot trajectory
+    plot(controller_mpc_cbf_one.xlog(1,:), controller_mpc_cbf_one.xlog(2,:), 'ko-',...
+        'LineWidth', 1.0,'MarkerSize',4);
+    % plot obstacle
+    pos = controller_mpc_cbf_one.obs.pos;
+    r = controller_mpc_cbf_one.obs.r;
+    th = linspace(0,2*pi*100);
+    x = cos(th) ; y = sin(th) ;
+    plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
+    plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
+    % plot target position
+    plot(controller_mpc_cbf_one.x0(1), controller_mpc_cbf_one.x0(2), 'db', 'LineWidth', 1);
+    plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+    h=get(gca,'Children');
+    h_legend = legend(h([end]), {'MPC-CBF ($N=1$)'}, 'Location', 'SouthEast');
+    set(h_legend, 'Interpreter','latex');
+    set(gca,'LineWidth', 0.2, 'FontSize', 15);
+    grid on
+    xlabel('$x (m)$','interpreter','latex','FontSize',20);
+    ylabel('$y (m)$','interpreter','latex','FontSize',20);
+    xticks(-5:0);
+    yticks(-5:0);
+    xlim([-5,0.2]);
+    ylim([-5,0.2]);
+    print(gcf, 'figures/mpc-cbf-avoidance-one-step.eps', '-depsc');
+    print(gcf, 'figures/mpc-cbf-avoidance-one-step.png', '-dpng', '-r800');
+end
+
+%% Simulate MPC-CBF with other N
+params_mpc_cbf.N = 8;
+if run_mpc_cbf_one
+    fprintf('Run MPC-CBF\n');
+    controller_mpc_cbf_multiple = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_multiple.obs = obs;
+    controller_mpc_cbf_multiple.sim(time_total);
+end
+
+%% Display MPC-CBF simulation with other N
+if display_mpc_cbf_one
+    % Plot simulation
+    figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+    set(gca,'LooseInset',get(gca,'TightInset'));
+    hold on;
+    % plot trajectory
+    plot(controller_mpc_cbf_multiple.xlog(1,:), controller_mpc_cbf_multiple.xlog(2,:), 'ko-',...
+        'LineWidth', 1.0,'MarkerSize',4);
+    % plot obstacle
+    pos = controller_mpc_cbf_multiple.obs.pos;
+    r = controller_mpc_cbf_multiple.obs.r;
+    th = linspace(0,2*pi*100);
+    x = cos(th) ; y = sin(th) ;
+    plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
+    plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
+    % plot target position
+    plot(controller_mpc_cbf_multiple.x0(1), controller_mpc_cbf_multiple.x0(2), 'db', 'LineWidth', 1);
+    plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+    h=get(gca,'Children');
+    h_legend = legend(h([end]), {'MPC-CBF ($N=8$)'}, 'Location', 'SouthEast');
+    set(h_legend, 'Interpreter','latex');
+    set(gca,'LineWidth', 0.2, 'FontSize', 15);
+    grid on
+    xlabel('$x (m)$','interpreter','latex','FontSize',20);
+    ylabel('$y (m)$','interpreter','latex','FontSize',20);
+    xticks(-5:0);
+    yticks(-5:0);
+    xlim([-5,0.2]);
+    ylim([-5,0.2]);
+    print(gcf, 'figures/mpc-cbf-avoidance-several-steps.eps', '-depsc');
+    print(gcf, 'figures/mpc-cbf-avoidance-several-steps.png', '-dpng', '-r800');
+end
+
+%% Simulate MPC-DC
+if run_mpc_dc
+    fprintf('Run MPC-DC\n');
+    controller_mpc_dc = MPC_DC(x0, system, params_mpc_dc);
+    controller_mpc_dc.obs = obs;
+    controller_mpc_dc.sim(time_total);
+end
+
+%% Display MPC-DC simulation
+if display_mpc_dc
+    % Plot simulation
+    figure('Renderer', 'painters', 'Position', [0 0 400 400]);
+    set(gca,'LooseInset',get(gca,'TightInset'));
+    hold on;
+    % plot trajectory
+    plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:), 'ko-',...
+        'LineWidth', 1.0,'MarkerSize',4);
+    % plot obstacle
+    pos = controller_mpc_dc.obs.pos;
+    r = controller_mpc_dc.obs.r;
+    th = linspace(0,2*pi*100);
+    x = cos(th) ; y = sin(th) ;
+    plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
+    plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
+    % plot target position
+    plot(controller_mpc_dc.x0(1), controller_mpc_dc.x0(2), 'db', 'LineWidth', 1);
+    plot(0.0, 0.0, 'dr', 'LineWidth', 1);
+    h=get(gca,'Children');
+    h_legend = legend(h([end]), {'MPC-DC ($N=8$)'}, 'Location', 'SouthEast');
+    set(h_legend, 'Interpreter','latex');
+    set(gca,'LineWidth', 0.2, 'FontSize', 15);
+    grid on
+    xlabel('$x (m)$','interpreter','latex','FontSize',20);
+    ylabel('$y (m)$','interpreter','latex','FontSize',20);
+    xticks(-5:0);
+    yticks(-5:0);
+    xlim([-5,0.2]);
+    ylim([-5,0.2]);
+    print(gcf, 'figures/mpc-dc-avoidance.eps', '-depsc');
+    print(gcf, 'figures/mpc-dc-avoidance.png', '-dpng', '-r800');
+end