image thumbnail

MPC Tutorial IV - State Space MPC with input increment

by

 

functions and simulink block implement state space MPC using input increment

f=mpcsetup(A,B,C,D,hP,hM,Q,R,x0,u0)
function f=mpcsetup(A,B,C,D,hP,hM,Q,R,x0,u0)
% MPCSETUP  Set-up State Space MPC controller
%
%   SSMPC=MPCSETUP(A,B,C,D,P,M,Q,R,X0,U0) returns a function handle for a
%   state space model predictive control for the state space model
%   x(k+1) = Ax(k) + Bu(k)
%   y(k)   = Cx(k) + Du(k)
%   with predictive horizon, P and moving horizon M, performance weights of
%   output, Q and of input, R, initial state, X0 and initial control, U0.
%
%   SSMPC is the online controller, which can be called as follows 
%
%   U = SSMPC(Y,REF) returns the control input, U (nu by 1) according to
%   current measured output, Y (1 by ny) and future reference, REF (nr by
%   ny, nr>=1). 
%
%   The MPC minimize the following performance criterion:
%
%   J = min Y'QY + U'RU
%
%   The online controller is implemented as a nested function so that the
%   state of the internal model can be kept inside of the function. This
%   simplifies the input and output interface of the online controller.
%
% See also: mpc, dmc

% Version 2.0 by Yi Cao at Cranfield University on 3 July 2011

% Example: 2-CSTR model
%{
% Six-state model
A=[ 0.1555  -13.7665   -0.0604         0         0         0
    0.0010    1.0008    0.0068         0         0         0
         0    0.0374    0.9232         0         0         0
    0.0015   -0.1024   -0.0003    0.1587  -13.6705   -0.0506
         0    0.0061         0    0.0006    0.9929    0.0057
         0    0.0001         0         0    0.0366    0.9398];
Bu=[0.0001       0
         0       0
   -0.0036       0
         0  0.0001
         0       0
         0 -0.0028];
Bd=[      0         0
          0         0
     0.0013         0
          0         0
          0         0
          0    0.0008];
C=[0 362.995 0 0 0 0
   0 0 0 0 362.995 0];
D=zeros(2,2);
% Prediction horizon and moving horizon
p=10;
m=3;
% Performance wights
Q=1.5*eye(2*p);
R=eye(2*m)*0.1;
% MPC set-up
ssmpc=mpcsetup(A,Bu,C,D,p,m,Q,R);
% Simulation length and variables for results
N=1500;
x0=zeros(6,1);
Y=zeros(N,2);
U=zeros(N,2);
% Predefined reference
T=zeros(N,2);
T(10:N,1)=1;
T(351:N,1)=3;
T(600:N,1)=5;
T(1100:N,1)=3;
T(210:N,2)=2;
T(551:N,2)=1;
T(800:N,2)=4;
T(1300:N,2)=1;
% Simulation
for k=1:N
    % Process disturbances
    w=Bd*(rand(2,1)-0.5)*2;
    % Measurements noise
    v=0.01*randn(2,1);
    % actual measurement
    y=C*x0+v;
    % online controller
    u=ssmpc(y,T(k:end,:)');
    % plant update
    x0=A*x0+Bu*u+w;
    % save results
    Y(k,:)=y';
    U(k,:)=u';
end
t=(0:N-1)*0.1;
subplot(211)
plot(t,Y,t,T,'--','linewidth',2)
title('output and setpoint')
ylabel('temp, C^\circ')
legend('T_1','T_2','T_1 Ref','T_2 Ref','location','south','Orientation','horizontal')
subplot(212)
stairs(t,U,'linewidth',2)
legend('u_1','u_2','location','southeast')
title('input')
ylabel('flow rate, m^3/s')
xlabel('time, s')
%}


% Input and output check
error(nargchk(8,10,nargin));
error(nargoutchk(1,1,nargout));

% dimension of the system
nx=size(A,1);
nu=size(D,2);

% precalculate the gain matrices of the online controller
[K1,F,Am,Bm,Cm] = predmat(A,B,C,D,hP,hM,Q,R);

% default initial conditions
if nargin<9
    x0=zeros(nx,1);
end
if nargin<10
    u0=zeros(nu,1);
end
xm=[x0;u0];

% the online controller
f = @ssmpc;
    function u=ssmpc(y,r)
        % update state
        xm = Am*xm + Bm*u0;
        % get the reference
        nr=size(r,2);
        if nr>=hP
            ref=r(:,1:hP);
        else
            ref=[r r(:,end+zeros(hP-nr,1))];
        end
        % model-plant mismatch
        offset = (y(:) - Cm*xm);
        % update optimal control
%         u0 = -K1*x0 + Pr*reshape(bsxfun(@minus,ref,offset),[],1);
        u0 = K1*(reshape(ref-offset(:,ones(hP,1)),[],1) - F*xm);
        % controller output
        u = u0+xm(end-nu+1:end);
    end
end

function [K1,F,Am,Bm,Cm] = predmat(A,B,C,D,hP,hM,Q,R)
 
%%%% Initialise
[nx,nu]=size(B);
ny = size(C,1);
Am = [A B; zeros(nu,nx) eye(nu,nu)];
Bm = [B; eye(nu,nu)];
Cm = [C D];

F = zeros(ny*hP,nx+nu);
F0 = Cm;
for i = 1:hP
    F0 = F0*Am;
    F((i-1)*ny+(1:ny),:) = F0;
end
H = zeros(ny*hP,nu*hM);
for i = 1:hP
    Hj = zeros(ny,nu*hM);
    Hi = Cm;
    for j = min(hM,i):-1:1
        Hj(:,(j-1)*nu+(1:nu)) = Hi*Bm;
        Hi = Hi*Am;
    end
    H((i-1)*ny+(1:ny),:) = Hj;
end
K=(H'*Q*H+R)\(H'*Q);
K1=K(1:nu,:);
end

Contact us