Constrained MPC with feed-forward SSTO
Table of Contents
- 1 Introduction
- 2 Pre-requisites
- 3 Source code
- 4 Isolated power system
- 5 State-space model of the system
- 6 Design requirements
- 7 MPC-SSTO tracking problem formulation
- 8 Cost function
- 9 Defining the inequality constraints
- 10 Target equilibrium optimization under constraints (SSTO)
- 11 Solving the LQ-MPC optimization problem
- 12 Stability
- 13 Algorithm
-
14 Simulation
- 14.1 Init
- 14.2 System: continuos to discrete
- 14.3 Constraints
- 14.4 Target equilibrium pairs and optimization under constraints
- 14.5 Constraints for SSTO model
- 14.6 Prediction, Constraint and Cost matrices
- 14.7 Check if closed-loop system is stable
- 14.8 Calculate the terminal cost matrix P
- 14.9 For loop
- 15 References
Introduction¶
A constrained Linear Quadratic Model Predictive Model Control (LQ-MPC) with offset-free tracking and disturbance rejection using the feed-forward Steady-State Target Optimization (SSTO) approach is presented in this paper. The MPC control law uses Finite-Receding-Horizon method in dual-mode. The stability and recursive feasibility are guaranteed by off-line dual-mode and on-line optimization. The results are presented by simulation.
Pre-requisites¶
- Control Systems theory.
- MPC theory.
- Previous post of constrained MPC.
- Convex quadratic optimization theory.
- Jupyter Notebook with Matlab-kernel.
- Matlab.
Source code¶
Isolated power system¶
The operation of an isolated power system under primary frequency control is modelled by the following block diagram.

In this system, a steam turbine produces mechanical power, which is subsequently converted to electrical power via a synchronous generator connected to the grid. A change in power demand, Δpd, causes the system frequency f[Hz] to change. The control objective is to drive frequency deviations, Δf, to zero following a demand change, Δpd. To aid this, a governor controls the steam flow input to the turbine in response to the error between the reference power Δpref and the regulated frequency Δω/R, where R>0 is the regulation factor.
The primary frequency control loop present in the system is, unfortunately, unable to regulate the frequency error to zero following demand changes. Therefore, the aim is to design a secondary frequency control loop that will adjust the reference power Δpref in response to frequency deviations Δω, in order to eliminate error and improve transient performance.
State-space model of the system¶
The isolated power system to be studied is a linear Single-Input Single-Output (SISO) model with the addition of the disturbance in the output. The linear continuous-time state-space model is,,
[Δ˙ωΔ˙pmΔ˙pv]=[−D/M1/M00−1/Tt−1/Tt−1/(RTg)0−1/Tg][ΔωΔpΔpv]+[001/Tg]Δpref+[−1/M00]ΔpdΔf=[5000][ΔωΔpΔpv]+ΔpdIn this model, the input, u, is the change in reference power to the turbine governor, i.e., Δpref (in per unit (p.u.) – that is, normalized with respect to a base value), and the output, y, is the frequency of the power system, i.e., Δf [Hz]. The states are the (deviations from operating points in) angular frequency, Δω, mechanical output power of the steam turbine, Δpm, and output power reference from the turbine governor, Δpv. The demand change Δpd is a disturbance, d. For the particular power system under consideration, the model parameters are: M=10, D=0.8, R=0.1, Tt=0.5, Tg=0.2
Design requirements¶
In response to a large step-change in demand—up to 0.3 p.u. in magnitude—the closed-loop system.
- Maintains stability.
- Frequency settles to |Δf|≤0.01[Hz] within 2 seconds of the disturbance initiation.
- Exhibits zero steady-state error in the output.
- Satisfies the constraints.
MPC-SSTO tracking problem formulation¶
The Feedfoward tracking approach by SSTO requires a target optimizer that calculates the steady-state values for the states xss and the input uss given a reference r in the presence of a disturbance d.

For the Discrete Linear Time-Invariant (DLTI) state-space system, x(k+1)=A x(k)+B u(k)+Bd d(k)y(k)=C x(k)+Dd d(k),k=0,1,2,…
The idea is to compute, x→xss,u→uss
Therefore, the objective is to regulate, ξ→0,v→0
Cost function¶
The problem IPN is to minimize v(k) given the following cost function, IPN(v(k)):minv(k)N−1∑j=0(ξ⊺(k+j|k) Q ξ(k+j|k)+v⊺(k+j|k) R v(k+j|k))+ξ⊺(k+N|k) P ξ(k+N|k)
subject to,
ξ(k+1+j|k)=A ξ(k+j|k)+B v(k+j|k)ξ(k|k)=ξ(k)Px(ξ(k+j|k)+xss)≤qxPu(v(k+j|k)+uss)≤quPxN ξ(k+N|k)≤˜qxNfor j=0,1,2,…,N−1.
Where Q and R are weight matrices, P is the terminal cost matrix, Px, Pu, PxN are the linear inequality (polyhedra) constraints, N is the finite-horizon. Note that Bd,Dd and d are not present because the SSTO will manage the disturbance rejection.
Defining the inequality constraints¶
Given |Δpref|=|u|≤umin,max, and |Δf|=|y|≤ymin,max, for y=C x, [C−C]x(k+j|k)≤[ymax−ymin][I−I]u(k+j|k)≤[umax−umin]
can be written in terms of ξ and v as follows,
[C−C]⏟Pxξ(k+j|k)≤[ymax−ymin]⏟qx−[C−C]⏟Pxxssand using a proper deadbeat mode-2 terminal inequality constraint to ensure a fast convergence to zero,
In×n⊗[PxPu K∞][(A+BK∞)0⋮(A+BK∞)N−1]⏟PxNξ(k+N|k)≤1n⊗[qx−Px xssqu−Pu uss]⏟˜qxNwhere K∞ is the deadbeat mode-2 gain calculated using Linear Quadratic Regulator (LQR) method.
Target equilibrium optimization under constraints (SSTO)¶
The offset-free equilibrium points (xss,uss) that satisfies the constraints exits if only if xss=Axss+Buss+Bdd and yss=Cxss+Ddd=r.
Thus, the SSTO problem is min{xss,uss}‖C xss+Dd d−r‖+ρ‖uss‖
subject to,
[I−A−BC0]⏟Aeq=T[xssuss]⏟xeq=[Bdr−Dd]d⏟beq[Px00Pu]⏟Aneq[xssuss]⏟xneq≤[qxqu]⏟bneqwhere ρ is a penalty constant.
Solving the LQ-MPC optimization problem¶
The solution of the LQ-MPC problem in compact form can written as, minv(k)12v⊺(k) H v(k)+ξ⊺(k) L⊺ v(k)+ξ⊺(k) M ξ(k)
The solution of (5) is the optimal control input sequence, v∗={v∗(k|k),v∗(k+1|k),…,v∗(k+N−1|k)}
Because MPC uses the receding-horizon principle, only the first optimized control input is needed v(k)=v∗(k|k)=κN(ξ(k))
Stability¶
In order to ensure stability it is necessary to compute a terminal cost matrix P that satisfies the Lyapunov equation (A+B K)⊺P(A+B K)−P+(Q+K⊺R K)=0
Algorithm¶
Now that the solution of the LQ-MPC is established and the stability can be guaranteed, there are basic conditions to meet such as
- (A,B) is stabilizable which can be proved by the controllability, in Matlab
rank(ctrb(A,B))
. - Q⪰0, can be C⊺C
- R≻0, changed later for tuning.

Simulation¶
Init¶
clear variables;
options = optimset('Display','off');
%% Global variables
x0 = [0; 0; 0];
r = 0.3; % reference
model_d = 1; % model with disturbance, 1=yes, 0=no
d = 0; % init value of the perturbation
N = 3; % Horizon increase N and cost value J(k) is reduced
nk = 80; % simulation steps
rho = 1; % SSTO optimization
% Q and R for tunning
Qdesign = 1;
Rdesign = .08;
System: continuos to discrete¶
Msys = 10; Dsys = 0.8; Rsys = 0.1; Tt = 0.5; Tg = 0.2;
Ac = [-Dsys/Msys 1/Msys 0 ;
0 -1/Tt 1/Tt ;
-1/(Rsys*Tg) 0 -1/Tg ];
Bc = [0; 0; 1/Tg];
if model_d==1
Bd = [-1/Msys; 0; 0;]; % Bd=disturbance matrix
else
Bd = [0; 0; 0];
end
C = [50, 0, 0];
D = 0;
Ts = 0.1; %sampling time
% Continuos + disturbance
sys = idss(Ac,Bc,C,D,Bd,x0,0);
% Converting to discrete model
sysd = c2d(sys,Ts);
A = sysd.A;
B = sysd.B;
C = sysd.C;
D = sysd.D;
Bd = -sysd.K;
if model_d == 1
Dd = 1;
else
Dd = 0;
end
Constraints¶
% Input constraints
u_max = 0.5; u_min = 0.5;
Pu = [1; -1];
qu = [u_max; u_min];
% State constraints
y_max = 0.5; y_min = 0.5;
Px = [eye(3).*C; -eye(3).*C];
qx = [ones(3,1)*y_max; ones(3,1)*y_min];
Target equilibrium pairs and optimization under constraints¶
% Target
T = [eye(3)-A, -B; C 0];
Tinv = T\[Bd*d; r-Dd*d];
xss = Tinv(1:3);
uss = Tinv(4);
% Optimization problem
x_optVect = [xss; uss]; % vector x for optimization problem A*x<b
fun = @(x_optVect) ( C(1)*x_optVect(1)+Dd*d-r ) + rho*x_optVect(4) ;
x0_opt = [r, 0, 0, 0]; % init values
% Equality constraints
Aeq = T;
beq = [Bd*d; r-Dd*d];
% Inequality constraints
Aopt = [Px, zeros(6,1); zeros(2,3), Pu];
bopt = [qx; qu];
% Minimization
[f,fval] = fmincon(fun,x0_opt,Aopt,bopt,Aeq,beq,[],[],[],options);
xss = f(1:3)'
uss = f(4)
yss = C*xss+Dd*d;
Constraints for SSTO model¶
% Input constraints
Pu_ssto = Pu;
qu_ssto = qu-Pu*uss;
% State constraints
Px_ssto = Px; %
qx_ssto = qx-Px*xss;
% Cost matrices
Q = (C'*C)*Qdesign; % Q^(1/2)
R = 1*Rdesign;
% Deadbeat Terminal Inequality State constraints
K = -dlqr(A,B,Q,R)
Maux = [];
for i =0:N-1
Maux = [Maux;(A+B*K)^(i)];
end
Mm = kron(eye(N),[Px; Pu*K]);
% tracking
PxN_ssto = Mm*Maux;
qxN_ssto = kron(ones(N,1),[qx-Px*xss; qu-Pu*uss]);% deadbeat terminal inequality constraints
Prediction, Constraint and Cost matrices¶
%%file 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO/predict_mats.m'
function [F,G] = predict_mats(A,B,N)
% P. Trodden, 2015.
% dimensions
n = size(A,1);
m = size(B,2);
% allocate
F = zeros(n*N,n);
G = zeros(n*N,m*(N-1));
% form row by row
for i = 1:N
% F
F(n*(i-1)+(1:n),:) = A^i;
% G
% form element by element
for j = 1:i
G(n*(i-1)+(1:n),m*(j-1)+(1:m)) = A^(i-j)*B;
end
end
end
cd 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO'
% Prediction matrices
[F,G] = predict_mats(A,B,N)
%%file 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO/constraint_mats.m'
function [Pc, qc, Sc, Pu_tilde, Px_tilde, qx_tilde, Px0_tilde] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf)
% P. Trodden, 2017.
% input dimension
m = size(Pu,2);
% state dimension
n = size(F,2);
% horizon length
N = size(F,1)/n;
% number of input constraints
ncu = numel(qu);
% number of state constraints
ncx = numel(qx);
% number of terminal constraints
ncf = numel(qxf);
% if state constraints exist, but terminal ones do not, then extend the
% former to the latter
if ncf == 0 && ncx > 0
Pxf = Px;
qxf = qx;
ncf = ncx;
end
%% Input constraints
% Build "tilde" (stacked) matrices for constraints over horizon
Pu_tilde = kron(eye(N),Pu);
qu_tilde = kron(ones(N,1),qu);
Scu = zeros(ncu*N,n);
%% State constraints
% Build "tilde" (stacked) matrices for constraints over horizon
Px0_tilde = [Px; zeros(ncx*(N-1) + ncf,n)];
if ncx > 0
Px_tilde = [kron(eye(N-1),Px) zeros(ncx*(N-1),n)];
else
Px_tilde = zeros(ncx,n*N);
end
Pxf_tilde = [zeros(ncf,n*(N-1)) Pxf];
Px_tilde = [zeros(ncx,n*N); Px_tilde; Pxf_tilde];
qx_tilde = [kron(ones(N,1),qx); qxf];
%% Final stack
if isempty(Px_tilde)
Pc = Pu_tilde;
qc = qu_tilde;
Sc = Scu;
else
% eliminate x for final form
Pc = [Pu_tilde; Px_tilde*G];
qc = [qu_tilde; qx_tilde];
Sc = [Scu; -Px0_tilde - Px_tilde*F];
end
end
cd 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO'
% Constructing Constraint matrices
[Pc_ssto,qc_ssto,Sc_ssto] = constraint_mats(F,G,Pu_ssto,qu_ssto,Px_ssto,qx_ssto,PxN_ssto,qxN_ssto)
Check if closed-loop system is stable¶
% Closed-loop
Acl = A+B*K;
% Check if it's stable
eigVal = eig(Acl);
if abs(real(eigVal)) <= 1
disp('Mode-2 CL system x(k+1) = (A+B*K)*x(k) is stable!');
else
disp('Mode-2 CL system x(k+1) = (A+B*K)*x(k) is unstable!');
end
disp('eigen values:');
disp(eigVal);
Calculate the terminal cost matrix P¶
Phi = A+B*K;
S = Q+K'*R*K;
P = dlyap(Phi',S)
%%file 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO/cost_mats.m'
function [H,L,M] = cost_mats(F,G,Q,R,P)
% P. Trodden, 2015.
% get dimensions
n = size(F,2);
N = size(F,1)/n;
% diagonalize Q and R
Qd = kron(eye(N-1),Q);
Qd = blkdiag(Qd,P);
Rd = kron(eye(N),R);
% Hessian
H = 2*G'*Qd*G + 2*Rd;
% Linear term
L = 2*G'*Qd*F;
% Constant term
M = F'*Qd*F + Q;
% make sure the Hessian is symmetric
H=(H+H')/2;
end
cd 'C:/Users/zurit/OneDrive/blog/files/MPC-SSTO'
% Cost matrices
[H,L,M] = cost_mats(F,G,Q,R,P)
For loop¶
% initialize
x = x0;
n = 3; m = 1;
xs = zeros(n,nk+1);
us = zeros(m,nk+1);
for k = 1:nk+1
% Function to call perturbation at specific time steps
if k == 30
d = 0.6;
elseif k == 60
d = -0.2;
else
d = 0;
end
ref(1,k) = r;
% Optimization problem
fun = @(x_optVect2) ( C(1)*x_optVect2(1)+Dd*d-r ) + (rho*x_optVect2(4)) ;
x0_opt = [x;us(1,k)];
% Equality constraints
Aeq = [eye(3)-A, -B ;C 0];
beq = [Bd*d; r-Dd*d];
% Minimization
[f,fval] = fmincon(fun,x0_opt,Aopt,bopt,Aeq,beq,[],[],[],options);
xss = f(1:3);
uss = f(4);
yss = C*xss+Dd*d;
x_optVect2 = [xss;uss]; % vector x for optimization problem A*x<b
% SSTO states
epsilon(:,k) = x-xss;
nu(:,k) = us(:,k)-uss;
% input constraints
qu_ssto = qu-Pu*uss;
% state constraints
qx_ssto = qx-Px*xss;
% deadbeat terminal inequality constraints
qxN_ssto = kron(ones(N,1),[qx-Px*xss; qu-Pu*uss]);
% Constraint matrices
[Pc_ssto,qc_ssto,Sc_ssto] = constraint_mats(F,G,Pu_ssto,qu_ssto,Px_ssto,qx_ssto,PxN_ssto,qxN_ssto);
% States
xs(:,k) = x;
% Measurement
y = C*xs+Dd*d;
% y1(k,:) = C*x+Dd*d; % violates y constraint |y|<0.5
[NUstar,fval,flag] = quadprog(H,L*epsilon(:,k),Pc_ssto,qc_ssto+Sc_ssto*epsilon(:,k),[],[],[],[],[],options);
% check feasibility
if flag < 1
disp(['Optimization is infeasible at k = ',num2str(k)]);
break;
end
% store input
us(:,k) = NUstar(1)+uss;
% Feedback x(k+1)=A*x(k)+B*u(k)
x = A*x+B*us(1,k)+Bd*d;
J(:,k) = 0.5*(NUstar(1:N,:)')*H*NUstar(1:N,:)+(epsilon(:,k)')*(L')*NUstar(1:N,:)+...
(epsilon(:,k)')*M*epsilon(:,k);
end
% Predicted states for ploting and for value function
xs_aux = xs';
xs1 = xs_aux(:,1)';
xs2 = xs_aux(:,2)';
xs3 = xs_aux(:,3)';
t = Ts*(0:nk); % simulation time
figure;
stairs(t,us(1,1:end)'); % constrained closed-loop
title(['Input u for N=',num2str(N)]);
xlabel('Time step k [s]');
ylabel('u(k)');
grid on;
figure;
plot(t,xs1,t,xs2,t,xs3); % constrained closed-loop
title(['States for N=',num2str(N)]);
xlabel('Time step k [s]');
ylabel('States');
legend('x1(k)','x2(k)','x3(k)','Location','Southeast');
grid on;
figure;
subplot(2,2,1);
hold on;
plot(xs1,xs2,'-o');
title(['phase-plot for N=',num2str(N)]);
xlabel('x1(k)');
ylabel('x2(k)');
grid on;
subplot(2,2,2);
hold on;
plot(xs1,xs3,'-o');
title(['phase-plot for N=',num2str(N)]);
xlabel('x1(k)');
ylabel('x3(k)');
grid on;
subplot(2,2,3);
hold on;
plot(xs2,xs3,'-o');
title(['phase-plot for N=',num2str(N)]);
xlabel('x2(k)');
ylabel('x3(k)');
grid on;
figure;
plot(t,y,t,ref);
title(['Output y for N=',num2str(N)]);
xlabel('Time step k [s]');
ylabel('y(k)');
legend('y(k)','r(k)');
grid on;
figure;
plot(t,J);
title(['Value function for N=',num2str(N)]);
xlabel('Time step k [s]');
ylabel('J(k)');
grid on;
References¶
[1] James B. Rawlings and Mayne David Q. Model Predictive Control Theory and Design. Nob Hill Pub, Llc, 2009. ISBN: 9780975937709.
[2] Jan Maciejowski. Predictive Control with Constraints. Prentice Hall, 2000. ISBN: 978-0201398236.
[3] P. Trodden, Lecture Notes ACS616 2018/19
[4] S. Dughman, J.A. Rossiter, A survey of guaranteeing feasibility and stability in MPC during target changes 9th International Symposium on Advanced Control of Chemical Processes, June 2015.
Comentarios