%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% Model Predictive Control (MPC).
%
% Constrained model predictive control is used in presence of saturation of
% the actuator. The system is excited by a white Gaussian noise d(t). The
% statistic of the velocity are computed and visualized in order to be
% compared to the other controlled cases.
% Actuator displecemnt is also implemented: to activate uncomment the
% respective code lines.
%
% Reference figure(s): Fig. 13, Fig. 14, Fig. 15
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nModel Predictive Control (MPC).\n\n')




%% Initialization of the plant --------------------------------------------

% LTI system matrix A and x grid
nq = 400; % number of grid points

[A,x,I] = KS_init(nq);


% Inputs matrix B

% disturbance d (Gaussian shape at x_d with sigma_d variance)
x_d = 35; sigma_d = 4; 
Bd  = exp(-((x-x_d)/sigma_d).^2)/sigma_d;

% actuator u (Gaussian shape at x_u with sigma_u variance)
x_u = 400; sigma_u = 4; 
Bu  = exp(-((x-x_u)/sigma_u).^2)/sigma_u;


% Outputs matrix C

% measurement y (Gaussian shape at x_y with sigma_y variance)
x_y = 300; sigma_y = 4; 
Cy  = (exp(-((x-x_y)/sigma_y).^2)/sigma_y).' * I;

% output z (Gaussian shape at x_z with sigma_z variance)
x_z = 700; sigma_z = 4; 
Cz  = (exp(-((x-x_z)/sigma_z).^2)/sigma_z).' * I;




%% Convolution kernels Pzu, Pzv --------------------------------------

% Parameters
dt = 1;
Tp = 1250; % prediction time-horizon

tp = 0:dt:Tp; ntp = length(tp);


% Time-discrete system (Eqn. 37-38)
Ad  = expm(A*dt); Bud = dt*Bu;
Cyd = Cy;
Czd = Cz;

% - memory saving
Ad = sparse(Ad .* (abs(Ad) > max(max(Ad))*1e-16/nq) );


% Convolution kernel Pqu (Eqn. 45)
Pvu = zeros(nq,ntp);

Pvu(:,1) = Bud;
for i = 2:ntp
    Pvu(:,i) = Ad*Pvu(:,i-1);
end


% Convolution kernels Pzu (Eqn. 48)
Pzu = Czd * Pvu;


% impulse response q(k) -> z(k+j) Pzv
Pzv = zeros(ntp,nq);

Pzv(1,:) = Cz;
for i = 2:ntp
    Pzv(i,:) = Pzv(i-1,:)*Ad;
end




%% MPC Matrices -----------------------------------------------------------

Wz = 1.0e+0; % z-output weight
Wu = 1.0e+0; % control penalty

Tc = 500;   % control time-horizon

tc = 0:dt:Tc; ntc = length(tc);


% Convolution operator (Eqn. 73)
PPzu = spdiags(kron(ones(ntp,1),Pzu(1,1:ntp)),-(1:ntp),ntp,ntc);


% Extended weight matrices (Eqn. 75)
WWz = kron(Wz,eye(ntp))/ntp;
WWu = kron(Wu,eye(ntc))/ntp;


% MPC matrices (Eqn. 78)
H = 2*(PPzu' * WWz * PPzu + WWu);
c = 2*  Pzv' * WWz * PPzu;


% Static gain (Eqn. 81)
KK = -H\c';
K  = KK(1,:);




%% Actuator displacement (uncomment to activate) --------------------------
% x_u = x_u + 5; 
% Bu = exp(-((x-x_u)/sigma_u).^2)/sigma_u;



  
%% Time integration -------------------------------------------------------

% Parameters
dt   = 1;
Tend = 50000;

Ton = 4000;

t  = 0:dt:Tend;
nt = length(t);


% Time-stepper matrices (Crank-Nicholson)
CNi = (speye(nq) - dt/2*A);
CNe = (speye(nq) + dt/2*A);


% Noise inizialization
d = randn(1,nt);     % unitary variance Gaussian white noise
d = d - mean(d,2);   % enforce zero-mean
d = d / std(d,[],2); % enforce unitary variance


% Variables initialization
v = zeros(nq,nt); % velocity
y = zeros(1 ,nt); % measurement signal
z = zeros(1 ,nt); % output signal
u = zeros(1 ,nt); % control signal

uu = zeros(ntc,1);% predicted control signal


% Optimisation toolbox setup
optmin = optimset('Algorithm','trust-region-reflective',...
                  'MaxIter'  ,100,...
                  'Display'  ,'off');


% Time-loop
for i = 1:nt-1
    
    % outputs
    y(:,i) = Cy*v(:,i);
    z(:,i) = Cz*v(:,i);
    
    if t(i) >= Ton
    % control forcing: constrained MPC (Eqn. 79)
        uu = quadprog(H,v(:,i)'*c,[],[],[],[],...
                                   -2*ones(ntc,1),2*ones(ntc,1),uu,optmin);
        u(:,i) = uu(1);
    
    % control forcing: unconstrained MPC (Eqn. 82) (uncomment to activate)
    %   u(:,i) = K*v(:,i);
    end
    
    % timestep
    v(:,i+1) = CNi\(CNe*v(:,i) + dt * (Bd*d(:,i) + Bu*u(:,i)));
    
    % runtime output
    runtime_output

end




%% Velocity statistics ----------------------------------------------------
Tsta = 20000;
ista = (Tend-Tsta <= t) & (t <= Tend);

% Root-Mean-Square
v_rms = sqrt( mean(v(:,ista).^2,2) - mean(v(:,ista),2).^2 );

% visualization
rms_style  = '-b';
rms_legend = 'MPC';

plot_rms




%% Figure 13 --------------------------------------------------------------
figure(13);

hold on; plot(t,u,'-b','Linewidth',1.5); hold off
axis([43500 45500 -3 3])
xlabel('t'); ylabel('u(t)'); grid on




%% Figure 15a -------------------------------------------------------------
figure(15); clf

Tcs = 750; 
tcs = 0:dt:Tcs; ntcs = length(tcs);

Hs  = 2*(PPzu(1:ntcs,:)' * WWz(1:ntcs,1:ntcs) * PPzu(1:ntcs,:) + WWu);
cs  = 2*  Pzv(1:ntcs,:)' * WWz(1:ntcs,1:ntcs) * PPzu(1:ntcs,:);
KKs = -Hs\cs';
Ks  = KKs(1,:);

subplot(2,1,1); plot(x,K ,'-b',...
                     x,Ks,'-r','Linewidth',1.5); hold on
                ax = axis; axis([0 800 ax(3:4)])
                xlabel('x'); ylabel('K(x)'); grid on
subplot(2,1,2); plot(tp,Pzu ,'-k','Linewidth',1.5); hold on
                plot([0 1250],-[0.325 0.325],'-b',...
                     [0  750],-[0.450 0.450],'-r','Linewidth',1.5);
                text(1250,-0.425,'T_P = 1250','HorizontalAlignment','Right')
                text( 750,-0.550,'T_P = 750' ,'HorizontalAlignment','Right')
                axis([0 Tp -0.6 0.4])
                xlabel('t'); ylabel('P_{zu}(t)'); grid on
