%Computes the log likelihood of Kalman filter for systems with lagged observables
function [LL]=KalmanLagLL(A,C,D1,D2,R,Z)

LL=0;
T=length(Z);
dimX=length(A);
dimZ=length(D1(:,1));
CC=C*C';

% P0=reshape(inv(eye(dimX*dimX)-kron(A,A))*CC(:),dimX,dimX)+eye(dimX)*1e-4;%Does the same thing as the next line if you don't have control toolbox
P0=dlyap(A,CC);
Xhat=zeros(dimX,T+1);
for tt=1:T
    Ztilde=Z(:,tt)-D1*A*Xhat(:,tt)-D2*Xhat(:,tt);
    Omega=(D1*A+D2)*P0*(D1*A+D2)'+(D1*C+R)*(D1*C+R)';
%     Omegainv=eye(dimZ)/(Omega+eye(dimZ)*1e-8); %Numerically more stable...usually not needed
    Omegainv=eye(dimZ)/Omega;
    K=(A*P0*(D1*A+D2)'+C*C'*D1'+C*R')*Omegainv;
    Xhat(:,tt+1)=A*Xhat(:,tt)+K*Ztilde;
    P1=A*P0*A'+C*C';
    P0=P1-K*Omega*K';
%     LL = LL - 0.5*(log(det(Omega+eye(dimZ)*1e-8)) +  Ztilde'*Omegainv*Ztilde); %Numerically more stable...usually not needed
    LL = LL - 0.5*(log(det(Omega)) + Ztilde'*Omegainv*Ztilde);
 end


if isreal(LL)==1
    LL=LL-0.5*dimZ*T*log(2*pi);
else
    LL=-(7e10);
     
end


    
    
    