function [llfn, klmG]=kalman_quwz(Q,U,W,Z, Y)
%Function to apply Kalman filter to the FRSW type model
%The model s(t+1)=Q s(t) +U e(t+1);
%          r(t+1)=W s(t) +Z e(t+1);
% Q: m*m, U: m*k, W: n*m, Z n*k
warning off;
pack;
%Output: llfn: the log likelihood function, klmG: the Kalman gain.
m=size(Q,1); %dimension of state
k=size(U,2); %dimension of shock;
n=size(W,1); %dimension of observables
nobs=size(Y,2);
if m~=size(Q,2);
    error('Q matrix is not square');
end;

if k~=size(Z,2);
    error('The size of U does not match that of Z');
end;

if n~=size(Z,1)
    error('W does not match Z');
end

if n~=size(Y,1);
    error('W does not match Y');
end;
%Initiation
 s_010=zeros(m,1);

 xx0=eye(m^2)-kron(Q,Q);
 xx1=vec(U*U');
 vecP010=xx0\xx1;
 P_010=reshape(vecP010,m,m);
% P_010=eye(m);
 if sum(find(real(eig(P_010))<=0))>=1
     P_010=eye(m,m);
 end;
 %P_010=ones(m,m);

 u=zeros(n,nobs);
 u(:,1)=Y(:,1); s_s1s=s_010; P_s1s=P_010;llfn0=zeros(nobs,1); %D_010=W*P_010*W'+Z*Z'; %D_t1s=D_010;
%Updating


for iter_n=1:nobs
    
    s_t1s=Q*s_s1s;
    r_t1s=W*s_s1s;
 %    P_t1s=Q*P_s1s*Q'+U*U';
    D_t1s=W*P_s1s*W'+Z*Z';        
    u(:,iter_n)=Y(:,iter_n)-r_t1s;
    kt=(Q*P_s1s*W'+U*Z')/(D_t1s);
 %    kt=(Q*P_010*W'+U*Z')/D_010;
    s_t1t=s_t1s+kt*u(:,iter_n);
   P_t1t=(Q-kt*W)*P_s1s*(Q'-W'*kt')+U*U'+kt*Z*(Z')*kt'-U*Z'*kt'-kt*Z*U'; %This calculation is based on (13.2.28) on Hamilton (p382)
  %P_t1t=Q*P_s1s*Q'+U*U'-kt*(W*P_s1s*Q'+Z*U');
    s_s1s=s_t1t;
    P_s1s=P_t1t;
    llfn0(iter_n)=-1/2*log(det(D_t1s))-1/2*u(:,iter_n)'/(D_t1s)*u(:,iter_n); %This is the true log-likelihood
%       disp('det(P)='),disp(det(P_t1s));
%      disp('det(D)='),disp(det(D_t1s));
%       disp('llfn0='),disp(llfn0(iter_n));
%      pause;
end  

llfn=sum(llfn0);

P_t1s=Q*P_s1s*Q'+U*U';
D_t1s=W*P_t1s*W'+Z*Z';
klmG=(Q*P_t1s*W'+U*Z')/(D_t1s);

%pause;
%Updating