11.1.1. Basic Example

Consider the following linear MPC problem with lower and upper bounds on state and inputs, and a terminal cost term:

\begin{align*} \text{minimize} \quad & x_N^\top P x_N + \sum_{i=0}^{N-1} x_i^\top Q x_i + u_i^\top R u_i \\ \text{subject to} \quad & x_0 = {\color{red} x} \\ & x_{i+1} = A x_i + B u_i \\ & \underline{x} \leq x_i \leq \bar{x} \\ & \underline{u} \leq u_i \leq \bar{u} \end{align*}

This problem is parametric in the initial state \(\color{red} x\) and the first input \(u_0\) is typically applied to the system after a solution has been obtained. The following code generates a function that takes \(-Ax\) as a calling argument and returns \(u_0\), which can then be applied to the system.

Here is the MATLAB code:

%% FORCES multistage form
% assume variable ordering zi = [u{i-1}, x{i}] for i=1...N

stages = MultistageProblem(N); % get stages struct of length N

for i = 1:N

        % dimension
        stages(i).dims.n = nx+nu; % number of stage variables
        stages(i).dims.r = nx;    % number of equality constraints
        stages(i).dims.l = nx+nu; % number of lower bounds
        stages(i).dims.u = nx+nu; % number of upper bounds

        % cost
        if( i == N )
                stages(i).cost.H = blkdiag(R,P); % terminal cost (Hessian)
        else
                stages(i).cost.H = blkdiag(R,Q);
        end
        stages(i).cost.f = zeros(nx+nu,1); % linear cost terms

        % lower bounds
        stages(i).ineq.b.lbidx = 1:(nu+nx); % lower bound acts on these indices
        stages(i).ineq.b.lb = [umin; xmin]; % lower bound for this stage variable

        % upper bounds
        stages(i).ineq.b.ubidx = 1:(nu+nx); % upper bound acts on these indices
        stages(i).ineq.b.ub = [umax; xmax]; % upper bound for this stage variable

        % equality constraints
        if( i < N )
                 stages(i).eq.C = [zeros(nx,nu), A];
        end
        if( i>1 )
                 stages(i).eq.c = zeros(nx,1);
        end
        stages(i).eq.D = [B, -eye(nx)];
end

% RHS of first eq. constr. is a parameter: stages(1).eq.c = -A*x0
params(1) = newParam('minusA_times_x0',1,'eq.c');

You can download the MATLAB code of this example to try it out for yourself by clicking here.

And here’s the Python code:

# FORCESPRO multistage form
# assume variable ordering zi = [u{i-1}, x{i}] for i=1...N

stages = forcespro.MultistageProblem(N) # get stages struct of length N

for i in range(N):

        # dimensions
        stages.dims[ i ]['n'] = nx+nu  # number of stage variables
        stages.dims[ i ]['r'] = nx     # number of equality constraints
        stages.dims[ i ]['l'] = nx+nu  # number of lower bounds
        stages.dims[ i ]['u'] = nx+nu  # number of upper bounds

        # cost
        if ( i == N-1 ):
                stages.cost[ i ]['H'] = np.vstack((np.hstack((R,np.zeros((nu,nx)))),np.hstack((np.zeros((nx,nu)),P))))
        else:
                stages.cost[ i ]['H'] = np.vstack((np.hstack((R,np.zeros((nu,nx)))),np.hstack((np.zeros((nx,nu)),Q))))
        stages.cost[ i ]['f'] = np.zeros((nx+nu,1)) # linear cost terms

        # lower bounds
        stages.ineq[ i ]['b']['lbidx'] = range(1,nu+nx+1) # lower bound acts on these indices
        stages.ineq[ i ]['b']['lb'] = np.concatenate((umin,xmin),0) # lower bound for this stage variable

        # upper bounds
        stages.ineq[ i ]['b']['ubidx'] = range(1,nu+nx+1)  # upper bound acts on these indices
        stages.ineq[ i ]['b']['ub'] = np.concatenate((umax,xmax),0) # upper bound for this stage variable

        # equality constraints
        if ( i < N-1 ):
                 stages.eq[i]['C'] = np.hstack((np.zeros((nx,nu)),A))
        if ( i>0 ):
                 stages.eq[i]['c'] = np.zeros((nx,1))
        stages.eq[i]['D'] = np.hstack((B,-np.eye(nx)))


# RHS of first eq. constr. is a parameter: stages(1).eq.c = -A*x0
stages.newParam('minusA_times_x0', [1], 'eq.c')
# define output of the solver
stages.newOutput('u0', 1, range(1,nu+1))

You can download the Python code of this example to try it out for yourself by clicking here.