%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% Kalman filter.
%
% A Kalman filter is designed and used to estimate the system state when
% excited by a white Gaussian noise d(t).
%
% Reference figure(s): Fig. 16, Fig. 17
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nKalman filter.\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;




%% Kalman filter ----------------------------------------------------------

Rd = 1.0e+0; % d variance
Rn = 1.0e-2; % n variance


% Solution of the estimation Riccati equation (Eqn. 91)
Re = care(A',Cy',Bd*Rd*Bd',Rn);


% Control gains matrix K (Eqn. 90)
L = - Re*Cy'/Rn;

        

        
%% 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);


% Noises 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

n = randn(1,nt);     % unitary variance Gaussian white noise
n = n - mean(n,2);   % enforce zero-mean
n = n./ std(n,[],2) * 1e-1;


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

v_obs = zeros(nq,nt); % compensator state
y_obs = zeros(1 ,nt); % estimated measurement signal
z_obs = zeros(1 ,nt); % estimated output signal


% Time-loop
for i = 1:nt-1
    
    % outputs
    y(:,i) = Cy*v(:,i) + n(:,i);
    z(:,i) = Cz*v(:,i);
    
    % timestep
    v(:,i+1) = CNi\(CNe*v(:,i) + dt * Bd*d(:,i));
    
    % Kalman filter
    if t(i) >= Ton
    % - estimated outputs
        y_obs(:,i) = Cy*v_obs(:,i);
        z_obs(:,i) = Cz*v_obs(:,i);
        
    % - timestep
        v_obs(:,i+1) = CNi\(CNe*v_obs(:,i) + dt * L *(y_obs(:,i)-y(:,i)));
    end
    
    % runtime output
    runtime_output

end




%% Estimation error -------------------------------------------------------
e = v - v_obs;

e_2     = sqrt(sum(e.^2,1)./sum(v.^2,1));
e_infty = max(abs(e),[],1)./max(abs(v),[],1);




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

plot(x,L,'-k','Linewidth',1.5);
ax = axis; axis([0 800 ax(3:4)]);
xlabel('x'); ylabel('L(x)'); grid on




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

subplot(7,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);
                  plot([Ton Ton], [0 800],'--b','Linewidth',2); hold off
                  axis xy; axis([0 10000 0 800]);
                  set(gca,'XTickLabel',[]); ylabel('x')
                  text((0+250),800-100,'plant','BackgroundColor',[1 1 1])
                  
                  caxis([-1 1]*20); cax = caxis;
                  colormap(jet(512));
                    
subplot(7,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(7,1,4:5); imagesc(t,x,v_obs); hold on
                  plot([0 Tend],[1 1]*x_y,'--k','Linewidth',2);
                  plot([Ton Ton], [0 800],'--b','Linewidth',2); hold off
                  axis xy; axis([0 10000 0 800]);
                  set(gca,'XTickLabel',[]); ylabel('x')
                  text((0+250),800-100,'observer','BackgroundColor',[1 1 1])
                  
                  caxis(cax); colormap(jet(512));
                  
subplot(7,1,6:7); semilogy(t,e_2    ,'-k',...
                           t,e_infty,'-b','LineWidth',1); hold on
                  plot([Ton Ton], [1e-2 1e0],'--b','Linewidth',2); hold on
                  axis([0 10000 1e-2 1e0]);
                  xlabel('t'), ylabel('z(t)'), grid on
                  xlabel('t'), ylabel('z(t)'), grid on
                  legend('||v_{obs} - v||_2 / ||v||_2',...
                         '||v_{obs} - v||_{\infty} / ||v||_{\infty}',...
                         'Orientation','Horizontal','Location','SO')
                  pos = get(gca,'Position'); 
                  pos(2) = pos(2) + pos(4)*2/5; pos(4) = pos(4)*3/5;
                  set(gca,'Position',pos);
