%UA, DEM, MIEM
%Nuno Silva, 51164
%30 de Março, 2011

%% Discrete Kalman Filter

function database=DKalmanF(N,database,modulo,sensor,eixo,A,H,R,Q,z)

I=eye(size(A));

%A posteriori estimate error covariance x-1
P_kmenos1=database.id.(['mod' num2str(modulo)]).(sensor).P(:,:,eixo);

if N==1
    %An estimate of the value of x-1
    x_kmenos1=database.id.(['mod' num2str(modulo)]).(sensor).Xest(:,1,eixo);
else
    %An estimate of the value of x-1
    x_kmenos1=database.id.(['mod' num2str(modulo)]).(sensor).Xest(:,N-1,eixo);
end

%A priori estimate of x
x_priori=A*x_kmenos1;

%A priori estimate error covariance
P_priori=A*P_kmenos1*A'+Q;

%Kalman gain
Kg=P_priori*H'*(H*P_priori*H'+R)^-1;
database.id.(['mod' num2str(modulo)]).(sensor).K(:,N)=Kg;

%An estimate of the value of x
x=x_priori+Kg*(z-H*x_priori);
database.id.(['mod' num2str(modulo)]).(sensor).Xest(:,N,eixo)=x;

%Estimate error covariance
P=(I-Kg*H)*P_priori;
database.id.(['mod' num2str(modulo)]).(sensor).P(:,:,eixo)=P;

end