%funzione Kalman smoother

function [x1, x2, x3, x4] = Esercitazione1Ex1e(A,C,Q,R,x0,P0,y1,y2,T)

x=zeros(4,T+1);
P=zeros(4,4,T);

Y=[y1; y2];

x(:, 1)= [x0(1) x0(2) x0(3) x0(4)]';
P(:, : ,1)=P0;

for t=1:T-1
    
    lambda=C*P(:,:,t)*C'+R;
   
    L=P(:,:,t)*C'*inv(lambda);
    
    K=A*L;
    
    x(:,t+1)=A*x(:,t) + K*(Y(:,t) - C*x(:,t));
    
    P(:,:,t+1)=A*(P(:,:,t)*A' -A* P(:,:,t)*C'*inv(lambda)*C*P(:,:,t))*A' + Q;
    
end

Ai=inv(A);
Qi=Ai*Q*(Ai)';



xi=zeros(4,T+1);
Pi=zeros(4,4,T);

Pzero=(1/1000)*eye(4);
Pi(:, : ,T)=Pzero;

xi(:, T)= [0 0 0 0]';

%Aggiornamento

for t=T-1:-1:1
    
    Pi(:,:,t)= Pi(:,:,t+1)+ C'*inv(R)*C;
    
    xi(:,t)= Pi(:,:,t+1)^(-1)*(Pi(:,:,t+1)*x(:,t+1)+ C'*inv(R)*Y(:,t));
      
end

%predizione

xtil=zeros(4,T+1);
Ptil=zeros(4,4,T);

for t=T-1:-1:1
    
    Ptil(:,:,t)= Ai*Pi(:,:,t+1)*Ai'+ Ai*Q*Ai';
    
    xtil(:,t)= Ai*xi(:,t+1);
      
end


P_tT=zeros(4,4,T);
x_tT=zeros(4,T+1);

P_tT(:,:,1)= (Pi(:,:,1)^(-1)+ P0^(-1))^-1;
x_tT(:,1)=P_tT(:,:,1)*(Pi(:,:,1)^(-1)*xi(:,1)+inv(P(:,:,1))*x(:,1)); 

for i=2:T
    
    P_tT(:,:,i)=inv((Pi(:,:,i))^(-1)+ inv(P(:,:,i)));

    
    x_tT(:,i)= P_tT(:,:,i)*(Pi(:,:,i)^(-1)*xi(:,i)+inv(P(:,:,i))*x(:,i));

   
end

x1=x_tT(1,:);
x2=x_tT(2,:);
x3=x_tT(3,:);
x4=x_tT(4,:);
