1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
| function mlogl = Kalman_filter(y,Z,D,VV,T,B,U,a0,P0,H,Q)
% -------------------------------------------------------------------------
% yt=Z*at+D*VVt H
% at=T*at-1+B*Ut Q
%--------------------------------------------------------------------------
%Exemple
%y = [ 1 6 ; 2 7 ; 3 12 ];
%Z = [ 1 1 1 0 0 ; -1 0 0 1 0 ];
%D = [ 1 0 ; 0 3 ];
%VV = [ 3 4 ; 1 6 ; 2 7 ];
%T = [ 1 0 0 0 0 ; 0 1 0 0 0; 0 0 0 0 0; 0 0 0 1 1; 0 0 0 0 1 ];
%B = [0 0 0 0 0 0 ; 1 1 0 0 0 1;0 0 1 1 0 1; 0 0 0 0 0 0; 0 0 0 0 1 0 ];
%U = [ 1 2 0 3 10 6 ; 3 4 2 1 1 2 ; 0 1 2 2 3 1 ];
%a0 = [ 0 ; 0 ;0 ;0;0];
%H = [ 0.1 0 ; 0 0.4 ];
%Q = [ 1 0 0 0 0 ; 0 1 0 0 0; 0 0 1 0 0; 0 0 0 1 0; 0 0 0 0 1];
%P0 = [ 0 0 0 0 0 ; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0; 0 0 0 0 0];
nobs = size(y,1); % rows
n = size(y,2); % cols
slogl = 0;
at = a0;
at_1 = a0;
Pt = P0;
Pt_1 = P0;
logl = zeros(nobs,1);
m=size(Z,2);
% placeholders
y_cond = zeros(nobs,n);
v = zeros(nobs,n);
a_cond = zeros(nobs,m);
a = zeros(nobs,m);
P_cond = zeros(nobs,m*m);
P = zeros(nobs,m*m);
F = zeros(nobs,n*n);
Zt=Z;
Dt=D;
Ht=H;
Tt=T;
Bt=B;
Qt=Q;
for i=1:nobs
yt=y(i,:)'; % y(t)
VVt=VV(i,:)';
Ut=U(i,:)';
% Prediction Equations
at_1 = Tt*at+Bt*Ut;
Pt_1 = Tt*Pt*Tt' + Qt;
% Innovations
yt_1 = Zt*at_1 + Dt*VVt ;
vt = yt-yt_1;
% Updating Equations
Ft = Zt*Pt_1*Zt' + Ht; % F(t)
inv_Ft = Ft\eye(size(Ft,1)); % Inversion de Ft
at = at_1 + Pt_1*Zt'*inv_Ft*vt ;
Pt = Pt_1 - Pt_1*Zt'*inv_Ft*Zt*Pt_1 ;
% Save results
y_cond(i,:) = yt_1';
v(i,:) = vt';
a(i,:) = at';
a_cond(i,:) = at_1';
P(i,:)=reshape(Pt,1,25);
P_cond(i,:)=reshape(Pt_1,1,25);
F(i,:)=reshape(Ft,1,4);
% Likelihood
dFt=det(Ft);
if dFt<=0
dFt=1e-10;
end
logl(i)=-(n/2)*log(2*pi)-0.5*log(dFt)-0.5*vt'*inv_Ft*vt;
slogl= slogl+logl(i);
end
mlogl=-slogl; |