Nonlinear model predictive control (regulation) in MATLAB with MPCTools

In this post we will attempt to create nonlinear model predictive control (MPC) code for the regulation problem (i.e., steering the state to a fixed equilibrium and keeping it there) in MATLAB using MPCTools. We will need MATLAB (version R2015b or higher), MPCTools¹ (a free Octave/MATLAB toolbox for nonlinear MPC), and CasADi² (version 3.1 or higher) (a free Python/MATLAB toolbox for nonlinear optimization and numerical optimal control). MPCTools calls Ipopt³ for solving the resulting nonlinear optimization problems. You can download the code created in this post here: regulation_NMPC_MPCTools.m.

We consider the following nonlinear MPC formulation:

\[\begin{aligned} \text{minimize} & \quad \int_{t}^{t+T_p}{l(x(\tau),u(\tau))d\tau} + V_f(x(t+T_p)) \\ \text{subject to} & \quad x(t) = \hat{x}(t) \\ & \quad \text{for } \tau \in [t, t+T_p]: \\ & \qquad \dot{x}(\tau) = f(x(\tau),u(\tau)) \\ & \qquad x_{\text{min}} \leq x(\tau) \leq x_{\text{max}} \\ & \qquad u_{\text{min}} \leq u(\tau) \leq u_{\text{max}} \\ & \quad e_f(x(t+T_p)) \leq 0, \end{aligned}\]

where \(T_p\) is the prediction horizon (in time units), \(l(\cdot)\) is the stage cost, \(x \in \mathbb{R}^{n_x}\) is the state vector, \(u \in \mathbb{R}^{n_u}\) is the control input vector, \(V_f(\cdot)\) is the terminal cost, \(\hat{x}(t)\) is the measurement, \(f(\cdot)\) is the dynamics, \(x_{\text{min}}\) and \(x_{\text{max}}\) are state constraints, \(u_{\text{min}}\) and \(u_{\text{max}}\) are control input constraints, while \(e_f\) is the terminal state constraint.

As an example, we take the following two dimensional system⁴:

\[\begin{aligned} \dot{x}_1(t) & = x_2(t) + u(t) \left( \mu + \left( 1 - \mu \right)x_1(t) \right) \\ \dot{x}_2(t) & = x_1(t) + u(t) \left( \mu - 4 \left( 1 - \mu \right)x_2(t)\right), \end{aligned}\]

where \(x_1(t) \in \mathbb{R}\) and \(x_2(t) \in \mathbb{R}\) are the state variables, \(u(t) \in \mathbb{R}\) is the control input, and \(\mu\) is a parameter we assume to be \(0.5\) here. In compact form we can write the dynamics as \(\dot{x}(t)=f(x(t),u(t))\), which we can define in code as follows:

function dxdt = define_dynamics(x, u)
    
    dxdt = [x(2) + u*(0.5 + 0.5*x(1));
        x(1) + u*(0.5 - 2*x(2))];
    
end

We specify various parameters and pre-allocate memory for the signals as follows:

function d = build_setup(d)
    
    % state constraints
    d.p.x_min = -Inf;
    d.p.x_max = Inf;
    
    % control input constraints
    d.p.u_min = -2;
    d.p.u_max = 2;
    
    % NMPC prediction horizon (in number of time steps)
    d.p.N_NMPC = 15;
    
    % sampling time (in time units)
    d.p.T = 0.1;
    
    % number of state variables
    d.p.n_x = 2;
    
    % number of control inputs
    d.p.n_u = 1;
    
    % simulation length (in number of time steps)
    d.p.t_final = 120;
    
    % pre-allocate memory
    d.s.x = NaN(d.p.n_x,d.p.t_final);
    d.s.u = NaN(d.p.n_u,d.p.t_final);
    
    % set initial state
    % (d.p.x0 is an argument of the main function)
    d.s.x(:,1) = d.p.x0;
    
    % state constraints vector
    d.p.x_min_v = d.p.x_min*ones(d.p.n_x,1);
    d.p.x_max_v = d.p.x_max*ones(d.p.n_x,1);
    
    % control input constraints vector
    d.p.u_min_v = d.p.u_min*ones(d.p.n_u,1);
    d.p.u_max_v = d.p.u_max*ones(d.p.n_u,1);
    
    % weighting matrices of stage cost
    d.p.Q = 0.5*eye(d.p.n_x);
    d.p.R = eye(d.p.n_u);
    
    % weighting matrix and bound for terminal cost and constraint
    d.p.P = [16.5926 11.5926;11.5926 16.5926];
    d.p.alpha = 0.7;
    
    global Q R P alpha
    Q = d.p.Q;
    R = d.p.R;
    P = d.p.P;
    alpha = d.p.alpha;
    
end

Here the use of -Inf and Inf indicates that the state is unconstrained, while the control input is constrained as \(-2 \leq u(t) \leq 2\).

We can create a simulator object, that will act as the plant within the simulation, as follows:

function d = create_simulator(d)

    d.c.simulator = ...
        d.c.mpc.getCasadiIntegrator(@define_dynamics, ...
        d.p.T, [d.p.n_x, d.p.n_u], {'x', 'u'});
    
end

Here the arguments are the dynamics \(f(\cdot)\), sampling time \(T\), and dimensions of the state and the control input. Note that, in the code, the structure d is the main structure containing everything, while the fields of d, namely p, s, and c contain parameters, signals, and controller objects, respectively.

We can build the nonlinear MPC with the following parts:

a) Import the dynamics \(f(\cdot)\):

ode_casadi_NMPC = d.c.mpc.getCasadiFunc(...
        @define_dynamics, ...
        [d.p.n_x, d.p.n_u], ...
        {'x', 'u'});

b) Specify how \(f(\cdot)\) should be discretized in time to get \(x(k+1) = F(x(k),u(k))\) by creating the function \(F(\cdot)\):

F = d.c.mpc.getCasadiFunc(...
        ode_casadi_NMPC, ...
        [d.p.n_x, d.p.n_u], ...
        {'x', 'u'}, ...
        'rk4', true(), ...
        'Delta', d.p.T);

Here the arguments are dimensions of the state and control input, whether to use an explicit Runge-Kutta method or not, via setting 'rk4' either true or false, and the timestep 'Delta'.

c) Considering a stage cost \(l(\cdot)\) in line with the regulation problem (which thus expresses the objective of keeping the state \(x(t)\) at the origin while also penalizing nonzero control inputs) as follows

\[\begin{equation} l(x(t),u(t)) = \left\lVert x(t) \right\rVert^2_Q + \left\lVert u(t) \right\rVert^2_R, \end{equation}\]

with

\[\begin{equation} Q = \begin{bmatrix} 0.5 ~ 0 \\ 0 ~ 0.5 \end{bmatrix} \qquad R = 1, \end{equation}\]

we create \(l(\cdot)\) in code as follows:

function l = define_stage_cost(x,u)
    
    global Q R
    
    l = x'*Q*x + u'*R*u;
    
end

d) Considering terminal cost \(V_f\) and terminal state constraint \(e_f\) (as, for example, required by the quasi-infinite horizon nonlinear MPC method⁴ for guaranteeing closed-loop stability) as follows:

\[\begin{align} V_f & = \left\lVert x \right\rVert^2_P \\ e_f & = x^T P x - \alpha \leq 0, \end{align}\]

with

\[\begin{equation} P = \begin{bmatrix} 16.5926 ~ 11.5926 \\ 11.5926 ~ 16.5926 \end{bmatrix} \qquad \alpha = 0.7, \end{equation}\]

we create \(V_f(\cdot)\) and \(e_f(\cdot)\) in code as follows:

function Vf = define_terminal_cost(x)
    
    global P
    
    Vf = x'*P*x;
    
end

function ef = define_terminal_constraint(x)
    
    global P alpha
    
    ef = x'*P*x - alpha;
    
end

We can finally create the nonlinear model predictive controller as follows:

function d = create_NMPC(d)
    
    % import dynamics
    ode_casadi_NMPC = d.c.mpc.getCasadiFunc(...
        @define_dynamics, ...
        [d.p.n_x, d.p.n_u], ...
        {'x', 'u'});
    
    % discretize dynamics in time
    F = d.c.mpc.getCasadiFunc(...
        ode_casadi_NMPC, ...
        [d.p.n_x, d.p.n_u], ...
        {'x', 'u'}, ...
        'rk4', true(), ...
        'Delta', d.p.T);
    
    % define stage cost
    l = d.c.mpc.getCasadiFunc(@define_stage_cost, ...
        [d.p.n_x, d.p.n_u], ...
        {'x', 'u'});
    
    % define terminal cost
    Vf = d.c.mpc.getCasadiFunc(@define_terminal_cost, ...
        d.p.n_x, {'x'}, {'Vf'});
    
    % define terminal state constraint
    ef = d.c.mpc.getCasadiFunc(@define_terminal_constraint, ...
        d.p.n_x, {'x'}, {'ef'});
    
    % define NMPC arguments
    commonargs.l = l;
    commonargs.lb.x = d.p.x_min_v;
    commonargs.ub.x = d.p.x_max_v;
    commonargs.lb.u = d.p.u_min_v;
    commonargs.ub.u = d.p.u_max_v;
    commonargs.Vf = Vf;
    commonargs.ef = ef;
    
    % define NMPC problem dimensions
    N.x = d.p.n_x; % state dimension
    N.u = d.p.n_u; % control input dimension
    N.t = d.p.N_NMPC; % time dimension (i.e., prediction horizon)
    
    % create NMPC solver
    d.c.solvers.NMPC = d.c.mpc.nmpc(...
        'f', F, ... % dynamics (discrete-time)
        'N', N, ... % problem dimensions
        'Delta', d.p.T, ... % timestep
        'timelimit', 1, ... % solver time limit (in seconds)
        '**', commonargs); % arguments
    
end

Furthermore, for calling the NMPC during simulation, we need

function d = solve_NMPC(d,t)
    
    % set state at time t as NMPC initial state
    d.c.solvers.NMPC.fixvar('x', 1, d.s.x(:,t));
    
    tic_c = tic;
    
    % solve NMPC problem
    d.c.solvers.NMPC.solve();
    
    % record CPU time
    d.s.CPU_time(t,1) = toc(tic_c);
    
    % assign first element of the solution to the NMPC
    % problem as the control input at time t
    d.s.u(:,t) = d.c.solvers.NMPC.var.u(:,1);
    
    % record the predicted state trajectory
    % for the first time step
    if t == 1
        
        d.p.x_NMPC_t_1 = d.c.solvers.NMPC.var.x;
        
    end
    
end

and to simulate the dynamical system we need

function d = evolve_dynamics(d,t)
    
    d.s.x(:,t+1) = ...
        full(d.c.simulator(d.s.x(:,t), d.s.u(:,t)));
    
end

All pieces are integrated in the function regulation_NMPC_MPCTools.m, which can be run by executing the command

d = regulation_NMPC_MPCTools(-0.7,-0.85)

from the MATLAB command window. The arguments here (i.e., \(-0.7\) and \(-0.85\)) are elements of the initial state vector. After the simulation is finished, the results should appear as a structure named d in the MATLAB workspace. A figure summarizing the results, including the state and control input trajectories, can be produced using the function plot_results_regulation_NMPC.m by executing the command

plot_results_regulation_NMPC(d)

from the MATLAB command window. The resulting figure is given below.

regulation NMPC with MPCTools, results

Risbeck, M. J., & Rawlings, J. B. (2016). MPCTools: Nonlinear model predictive control tools for CasADi. ↩
Andersson, J. A., Gillis, J., Horn, G., Rawlings, J. B., & Diehl, M. (2018). CasADi: a software framework for nonlinear optimization and optimal control. Mathematical Programming Computation, 1-36. ↩
Wächter, A., & Biegler, L. T. (2006). On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming. Mathematical programming, 106(1), 25-57. ↩
Chen, H., & Allgöwer, F. (1998). A Quasi-Infinite Horizon Nonlinear Model Predictive Control Scheme with Guaranteed Stability. Automatica, 34(10), 1205-1217. ↩ ↩²

Işık İlber Sırmatel

Nonlinear model predictive control (regulation) in MATLAB with MPCTools

Leave a Comment

You May Also Enjoy

EEE126 Linear Algebra

EEE462 Model Predictive Control

EEE461 Optimization

EEE314 Automatic Control Systems