Simple MPC Example

/ / /

Simple MPC 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} \ & x_N^T P x_N + \sum_{i=0}^{N-1} x_i^T Q x_i + u_i^T R u_i \\ \text{subject to} \ & x_0 = \color{#d22d33}{x} \\ & x_{i+1} = Ax_i + Bu_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{#d22d33}{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:

Matlab
Python

%% FORCES multistage form % assume variable ordering zi = [ui, xi+1] for i=1...N-1 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');

Download MATLAB code with example simulation

# FORCES multistage form # assume variable ordering zi = [ui, xi+1] for i=1...N-1 stages = MultistageProblem(N) # get stages struct of length N for i in range(N): # dimension 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))

Download Python code with example simulation

About Us

embotech, short for embedded optimization technologies, was founded in September 2013. We strive to lead the market for numerical optimization solutions that will be integral part of tomorrow's decision making systems. The company is a spin-off of the Automatic Control Laboratory from ETH Zurich.

Contact Us

We are located at Technopark Zurich, 10 minutes from Zurich main station and 20 minutes from Zurich International Airport.

		Technoparkstrasse 1 CH-8005 Zurich
		info@embotech.com
		CHE-195.606.420

Simple MPC Example