function est = kest_rec2(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t)

% RECURSIVE
fprintf('Recursive Kalman filtering\n')
fprintf('Compute off-line paramenter...')

% Allocate memory
[outs,states] = size(C);
P_prior = zeros(states,states,N); 
P_post = P_prior;
G = zeros(outs,outs,N);
iG = G;
L = zeros(states,outs,N);
e_prior=zeros(M,4,N);
e_post=zeros(M,4,N);

% Initialize
P_prior(:,:,1) = P0;

% Compute off-line parameters
for j = 1:N    
    G(:,:,j) = C*P_prior(:,:,j)*C' + R;    
    iG(:,:,j) = pinv( G(:,:,j) );
    L(:,:,j) = P_prior(:,:,j)*C'*iG(:,:,j); 
    P_post(:,:,j) = P_prior(:,:,j) - L(:,:,j)*C*P_prior(:,:,j);
    P_prior(:,:,j+1) = A * P_post(:,:,j) * A' + Q;
end

fprintf('done\n')

fprintf('Compute estimate...')
for m=1:M
    [y,t,x] = lsim(sys,squeeze(u(m,:,:)),t,x0); y = y'; x = x';
    x_post = zeros(states,N);
    x_prior = zeros(states,N+1);
    x_prior(:,1) = x0_;
    % Compute estimate
    for j = 1:N
        x_post(:,j) = x_prior(:,j) + L(:,:,j) * ( y(:,j) - C*x_prior(:,j) );  
        x_prior(:,j+1) = A*x_post(:,j);
    end
    y_ = C * x_post;
    e_prior(m,:,:)=x-x_prior(:,1:end-1); %Errore di stima a priori
    e_post(m,:,:)=x-x_post(:,1:end); %Errore di stima a posteriori
end
fprintf('done\n')

est.x=x;
est.y=y;
est.P_prior = P_prior;
est.P_post = P_post;
est.L = L;
est.x_prior = x_prior;
est.x_post = x_post;

e=squeeze(e_prior(M,:,:));
% Calcolo della varianza campionaria dell'errore di predizione tramite algoritmo NON RICORSIVO
est.P_emp=zeros(4,4,N);
for j = 1:N
     X=e(:,1:j)'-ones(j,1)*mean(e(:,1:j)',1);
     est.P_emp(:,:,j)=(X'*X)./j;
end
% Calcolo della varianza campionaria dell'errore di predizione tramite algoritmo RICORSIVO 
est.P_emp2 = zeros(4,4,N);
AVG=e(:,1);
for j = 2:N
     est.P_emp2(:,:,j)= ((j-1)/j)*est.P_emp2(:,:,j-1)+((j-1)/(j*j))*(AVG-e(:,j))*(AVG-e(:,j))';
     AVG=((j-1)/j)*AVG+e(:,j)./j;
end

e=squeeze(e_post(M,:,:));
% Calcolo della varianza campionaria dell'errore di filtraggio tramite algoritmo NON RICORSIVO 
est.P_emp_post=zeros(4,4,N);
for j = 1:N
     X=e(:,1:j)'-ones(j,1)*mean(e(:,1:j)',1);
     est.P_emp_post(:,:,j)=(X'*X)./j;
end
% Calcolo della varianza campionaria dell'errore di filtraggio tramite algoritmo RICORSIVO 
est.P_emp_post2 = zeros(4,4,N);
AVG=e(:,1);
for j = 2:N
    est.P_emp_post2(:,:,j)= ((j-1)/j)*est.P_emp_post2(:,:,j-1)+((j-1)/(j*j))*(AVG-e(:,j))*(AVG-e(:,j))';
    AVG=((j-1)/j)*AVG+e(:,j)./j;
end

% Calcolo della varianza campionaria dell'errore di predizione SU M SIMULAZIONI tramite algoritmo NON RICORSIVO 
est.PM_prior=zeros(4,4,N);
for j = 1:N
     X=squeeze(e_prior(:,:,j))-ones(M,1)*mean(squeeze(e_prior(:,:,j)),1);
     est.PM_prior(:,:,j)=(X'*X)./M;
end

% Calcolo della varianza campionaria dell'errore di filtraggio SU M SIMULAZIONI tramite algoritmo NON RICORSIVO 
est.PM_post=zeros(4,4,N);
for j = 1:N
     X=squeeze(e_post(:,:,j))-ones(M,1)*mean(squeeze(e_post(:,:,j)),1);
     est.PM_post(:,:,j)=(X'*X)./M;
end

% Calcolo della traccia delle matrici di varianza d'errore
est.NP_emp=zeros(N,1);
est.NP_emp2=zeros(N,1);
est.NP_emp_post=zeros(N,1);
est.NP_emp_post2=zeros(N,1);
est.NP=zeros(N,1);
est.NP_post=zeros(N,1);
est.NPM_prior=zeros(N,1);
est.NPM_post=zeros(N,1);
for j = 1:N
    est.NP_emp(j)=trace(est.P_emp(:,:,j));
    est.NP_emp2(j)=trace(est.P_emp2(:,:,j));
    est.NP(j)=trace(est.P_prior(:,:,j));
    est.NP_emp_post(j)=trace(est.P_emp_post(:,:,j));
    est.NP_emp_post2(j)=trace(est.P_emp_post2(:,:,j));
    est.NP_post(j)=trace(est.P_post(:,:,j));
    est.NPM_prior(j)=trace(est.PM_prior(:,:,j));
    est.NPM_post(j)=trace(est.PM_post(:,:,j));
end

%[est.P,eigenvalues,L] = dare(A',C',Q,R); L = L';
est.P=P_prior(:,:,1);

