%--------------------------------------------------------------------------
%
% Kuramoto Sivashinsky equation
%
% LMS filter.
%
% A LMS filter is used to identify the FIR-kernel Ezy. Then, the resulting
% kernel is compared with the Kalman filter solution.
%
% Reference figure(s): Fig. 18, Fig. 19 
% 
% Nicolo' Fabbiane, March 2014
% KTH Mechanics
% nicolo@mech.kth.se
%
%--------------------------------------------------------------------------
clc, clear all
fprintf('\nLMS 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 (reference) ----------------------------------------------

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;


% External description
dt   = 1;
Tker = 1500;

tker = 0:dt:Tker;
nker = length(tker);

% - time-discrete system (Eqn. 37-38)
Aed  = expm((A + L*Cy)*dt);
Czd = Cz;

% - Convolution kernel Ezy (Eqn. 97)
Evy = zeros(nq,nker);

Evy(:,1) = -L * dt;
for i = 2:nker
    Evy(:,i) = Aed*Evy(:,i-1);
end

Ezy = Czd * Evy;

% - FIR reduction  (Eqn. 98)
toll = 1e-2; % kernel reduction tollerance
ifir = (find( abs(Ezy) > max(abs(Ezy))*toll ,1,'first')) : ...
       (find( abs(Ezy) > max(abs(Ezy))*toll ,1,'last'));
   
Ezy_fir = Ezy(ifir);

% - FIR kernel length
nfir = length(Ezy_fir);


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

z_lms   = zeros(1,nt);        % estimated output signal
e_lms   = zeros(1,nt);        % estimation error
Ezy_lms = zeros(1,nfir,nt);   % identified convolution kernel
y_buf   = zeros(1,ifir(end)); % measurement signal buffer
mu      = zeros(1,nt);        % step length
lambda  = zeros(1,nfir);      % step direction


% Time-loop
for i = 1:nt-1
    
    % outputs
    y(:,i) = Cy*v(:,i) + n(:,i);
    z(:,i) = Cz*v(:,i);
    
    % update measurement buffer
    y_buf = [y(:,i) y_buf(:,1:end-1)];
    
    % timestep
    v(:,i+1) = CNi\(CNe*v(:,i) + dt * Bd*d(:,i));
    
    % LMS filter
    if t(i) >= Ton
    % - estimated output (Eqn. 98)
        z_lms(:,i) = Ezy_lms(1,:,i) * y_buf(ifir)';
        
    % - estimation error (Eqn. 99)
        e_lms(:,i) = z_lms(:,i) - z(:,i);
        
    % - update direction (Eqn. 102)
        lambda = -2 * e_lms(:,i) * y_buf(ifir);
        
    % - step length (Eqn. 107-108)
        mu(i) = - e_lms(:,i) / (lambda * y_buf(ifir)');
        mu(i) = min([mu(i), 2/var(y_buf)]);
        
    % - kernel update (Eqn. 101)
        Ezy_lms(1,:,i+1) = Ezy_lms(1,:,i) + mu(i)*lambda;
    end
    
    % runtime output
    runtime_output

end




%% Identification error ---------------------------------------------------
E = squeeze(Ezy_lms) - kron(Ezy_fir', ones(1,nt));

E_2     = sqrt(sum(E.^2,1)./sum(Ezy_fir.^2));
E_infty = max(abs(E),[],1)./max(abs(Ezy_fir));




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

plot(tker(ifir),Ezy_fir         ,'or',...
     tker(ifir),Ezy_lms(1,:,end),'sb','MarkerSize',5);
ax = axis; axis([tker(ifir(1)) tker(ifir(end)) ax(3:4)])
legend('E_{zy} FIR','E_{zy} FIR id. via LMS')
xlabel('i \Delta t'); ylabel('E_{zy}(i)'); grid on




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

subplot(5,1,2:3); imagesc(t,tker(ifir),squeeze(Ezy_lms)); hold on
                  plot([Ton Ton], t(ifir([1,end])),'--b','Linewidth',2); hold off
                  axis xy; axis([0 20000 t(ifir([1,end]))]);
                  set(gca,'XTickLabel',[]); ylabel('i \Delta t')
                  
                  cax = caxis; caxis([-1,1]*max(abs(cax))); cax = caxis;
                  colormap(jet(512));
                    
subplot(5,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,'E_{zy, LMS}(i)')
                  set(hc,'Position',pos);
                  
subplot(5,1,4:5); semilogy(t,E_2    ,'-k',...
                           t,E_infty,'-b','LineWidth',1); hold on
                  plot([Ton Ton], [1e-1 1e0],'--b','Linewidth',2); hold on
                  axis([0 20000 1e-1 1e0]);
                  xlabel('t'), ylabel('z(t)'), grid on
                  legend('||E_{zy, LMS}(i) - E_{zy}(i)||_2 / ||E_{zy}(i)||_2',...
                         '||E_{zy, LMS}(i) - E_{zy}(i)||_{\infty} / ||E_{zy}(i)||_{\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);
