%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% Response to a white Gaussian disturbance d(t)
%
% A white noise signal is considered as input d(t) and the time-response of
% the plant is calculated. Later, the statistics of the velocity are 
% computed and visualized to be compared with the controlled cases.
%
% Reference figure(s): Fig. 6, Fig. 14, Fig. 22, Fig. 23
%
% APPENDIX: In the last part of the script, time correlations, spectra and
% probability density functions of the input/output signals are also
% computed and visualized.
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nResponse to a white Gaussian disturbance d(t)\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 = 300; 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;

        

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

% Parameters
dt   = 1;
Tend = 50000;

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


% Time-loop
for i = 1:nt-1
    
    % outputs
    y(:,i) = Cy*v(:,i);
    z(:,i) = Cz*v(:,i);
    
    % timestep
    v(:,i+1) = CNi\(CNe*v(:,i) + dt * Bd*d(:,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  = '-k';
rms_legend = 'uncontrolled';

plot_rms




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

% Mean and variance
d_avg = mean(d(ista),2);
d_var = mean((d(ista)-d_avg).^2,2);
d_std = sqrt(d_var);

y_avg = mean(y(ista));
y_var = mean((y(ista)-y_avg).^2,2);
y_std = sqrt(y_var);

z_avg = mean(z(ista));
z_var = mean((z(ista)-z_avg).^2,2);
z_std = sqrt(z_var);




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

subplot(6,1,2:3); imagesc(t,x,v); hold on
                  plot([0 Tend],[1 1]*x_y,'-k',...
                       [0 Tend],[1 1]*x_z,'-k','Linewidth',2); hold off
                  axis xy; axis([0 5000 0 800]);
                  set(gca,'XTickLabel',[]); ylabel('x')
                  
                  caxis([-1 1]*20); cax = caxis;
                  colormap(jet(512));
                    
subplot(6,1,1);   caxis(cax); colormap(jet(512));
                  pos = get(gca,'Position'); pos(4) = pos(4)/3;
                  set(gca,'Visible','off','Position',pos)
                  hc = colorbar('NO'); xlabel(hc,'v(x,t)')
                  set(hc,'XTick',cax(1):5:cax(2),...
                         'XTickLabel',cax(1):5:cax(2),'Position',pos);
                  
subplot(6,1,4);   plot(t,d,'-k','Linewidth',1); hold on
                  plot([0 Tend], [1 1]*d_std,'--r',...
                       [0 Tend],-[1 1]*d_std,'--r','Linewidth',1); hold off
                  axis([0 5000 [-1 1]*3*d_std]);
                  set(gca,'XTickLabel',[]); ylabel('d(t)'), grid on
                  
subplot(6,1,5);   plot(t,y,'-k','Linewidth',1); hold on
                  plot([0 Tend], [1 1]*y_std,'--r',...
                       [0 Tend],-[1 1]*y_std,'--r','Linewidth',1); hold off
                  axis([0 5000 [-1 1]*3*y_std]);
                  set(gca,'XTickLabel',[]); ylabel('y(t)'), grid on
                  
subplot(6,1,6);   plot(t,z,'-k','Linewidth',1); hold on
                  plot([0 Tend], [1 1]*z_std,'--r',...
                       [0 Tend],-[1 1]*z_std,'--r','Linewidth',1); hold off
                  axis([0 5000 [-1 1]*3*z_std]);
                  xlabel('t'), ylabel('z(t)'), grid on




%% Other signals statistics (Appendix) ------------------------------------

% Time correlation - R_w(tau) = 1/T * int w(t)*w(t-tau) dt
Tcor = 500; % tau span

tau  = -Tcor:dt:Tcor;
ntau = length(tau);

% - correlation calc
R_d = zeros(1,ntau);
R_y = zeros(1,ntau);
R_z = zeros(1,ntau);

icor = (Tend-Tsta <= t) & (t <= Tend-Tcor);
for i = 1:ntau
    ishift = (Tend-Tsta + tau(i) <= t) & (t <= Tend-Tcor + tau(i));
    
    R_d(i) = mean(d(icor) .* d(ishift));
    R_y(i) = mean(y(icor) .* y(ishift));
    R_z(i) = mean(z(icor) .* z(ishift));
end

%-visualization
figure(1002); clf
plot(tau, R_d/d_var,'-r',...
     tau, R_y/y_var,'-g',...
     tau, R_z/z_var,'-b','Linewidth',1.5)
 
xlabel('\tau'), ylabel('R(\tau) / variance'), grid on
legend('R_d','R_y','R_z','Location','NW')



% Spectrum - Phi(omega) = Fou( R(tau) )
omega = (2*pi)/(2*Tcor) * (tau/dt);

% - spectra calc
Phi_d = fftshift(fft(R_d));
Phi_y = fftshift(fft(R_y));
Phi_z = fftshift(fft(R_z));

%-visualization
iplot = (omega>=0); % positive freq. only for symmetry
figure(1003); clf
loglog(omega(iplot)/(2*pi), abs(Phi_d(iplot))/d_var,'-r',...
       omega(iplot)/(2*pi), abs(Phi_y(iplot))/y_var,'-g',...
       omega(iplot)/(2*pi), abs(Phi_z(iplot))/z_var,'-b','Linewidth',1.5)
 
xlabel('f = \omega / 2\pi'), ylabel('|\Phi(\omega)| / variance'), grid on
legend('\Phi_d','\Phi_y','\Phi_z','Location','NW')



% Probability density function (pdf)
nint = 50; % number of intervals

% - defining the intervals based on the variance of each signal
dint = linspace(-4,4,nint+1) * d_std;
yint = linspace(-4,4,nint+1) * y_std;
zint = linspace(-4,4,nint+1) * z_std;

% - interval witdth
dd = dint(2)-dint(1);
dy = yint(2)-yint(1);
dz = zint(2)-zint(1);

% - pdf calc
pdf_d = zeros(1,nint);
pdf_y = zeros(1,nint);
pdf_z = zeros(1,nint);

for i = 1:nint;
    pdf_d(i) = mean((dint(i) <= d(ista)).*(d(ista) < dint(i+1)))/dd;
    pdf_y(i) = mean((yint(i) <= y(ista)).*(y(ista) < yint(i+1)))/dy;
    pdf_z(i) = mean((zint(i) <= z(ista)).*(z(ista) < zint(i+1)))/dz;
end

% - visualization
dint_plot = (dint(1:end-1) + dint(2:end))/2;
yint_plot = (yint(1:end-1) + yint(2:end))/2;
zint_plot = (zint(1:end-1) + zint(2:end))/2;

figure(1004); clf
plot(dint_plot/d_std, pdf_d*d_std,'-r',...
     yint_plot/y_std, pdf_y*y_std,'-g',...
     zint_plot/z_std, pdf_z*z_std,'-b',...
     dint_plot/d_std, normpdf(dint_plot/d_std,0,1),'--k','Linewidth',1.5)
 
xlabel('signal value / standard dev.'), ylabel('pdf * standard dev.'), grid on
legend('d','y','z','Gaussian (reference)','Location','NW')