%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% Controllability and observability Gramians.
%
% Controllability and observability Gramians are computed.
%
% Reference figure(s): Fig. 8
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nControllability and observability Gramians.\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;




%% Controllability and observability Gramian ------------------------------

% Solution of the Lyapunov equation (Eqn. 56)
Gcu  = lyapchol(A ,Bu ); Gcu = Gcu'*Gcu;

% Solution of the Lyapunov equation (Eqn. 57)
Goy  = lyapchol(A',Cy'); Goy = Goy'*Goy;





%% Figure 8 ---------------------------------------------------------------
figure(8); clf

Gplot = triu(Gcu)/trace(Gcu) + ...
        tril(Goy)/trace(Goy) ;

imagesc(x,x,log10(abs(Gplot))); hold on
plot([0 800],[0 800],'-w','LineWidth',3)
axis equal, axis([0 800 0 800]); ax = axis;
xlabel('x'); ylabel('x');

plot([1 1]*x_u,[0 1]*x_u,'--w',...
     [0 1]*x_y,[1 1]*x_y,'--w','LineWidth',1)

caxis([-6 -2]); cax = caxis;
hh = colorbar('YTick',cax(1):cax(2),...
              'YTickLabel',num2str(10.^(cax(1):cax(2))','%0.0e'));
ylabel(hh,'G')
