% Implementazione equazioni di Kalman

function [x1, x2, x3, x4] = Esercitazione1Ex1b(A,C,Q,R,x0,P0,Y1,Y2,T)

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

%x(t|t-1)
x_pred=zeros(4,T+2);
%P(t|t-1)
P_pred=zeros(4,4,T+2);

Y=[Y1; Y2];


%Condizioni iniziali per algoritmo di Kalman
x_pred(:, 1)= [x0(1) x0(2) x0(3) x0(4)]';
P_pred(:, : ,1)=P0;

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

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