function [x1,p1,x2,p2,llf]=kfilt_rdiag_llf(y,x1,p1,h,f,rdiag,q)
%adapted from npi_em_4.gss
%kalman filter procedure--hamilton notation
% y(t)=h'x(t)+w(t)
% x(t)=f x(t-1)+v(t)
%
% var(w(t)) = r;  Diagonal in this program, diagonal element is used
% var(v(t))=q;
%
% x1=x(t-1/t-1)--on input
% p1=p(t-1/t-1)--on input
% output includes innovations,variance of innovations & log-likelihood;


x2=f*x1;
e=y(1)-h(:,1)'*x2;
p2=f*p1*f'+q;
ht=h(:,1)'*p2*h(:,1)+rdiag(1);
k=p2*h(:,1)/ht;
x1=x2+k*e;
p1=(eye(size(p1,1))-k*h(:,1)')*p2;
%compute log-likelihood%...
llf=-0.5*(log(det(ht))+(e'/ht*e));


for i=2:size(y,1)
    e=y(i)-h(:,i)'*x1;
    ht=h(:,i)'*p1*h(:,i)+rdiag(i);
    k=p1*h(:,i)/ht;
    x1=x1+k*e;
    p1=(eye(size(p1,1))-k*h(:,i)')*p1;
    %compute log-likelihood%...
    llf=llf-0.5*(log(det(ht))+(e'/ht*e));
end

end
