function [shatnew,signew,lh,yhat]=kf(y,H,shat,sig,G,M)
%function [shatnew,signew,lh,yhat]=kf(y,H,shat,sig,G,M)
% s is the state, and the plant equation is s(t)=Gs(t-1)+Me, where e is
% N(0,I).  The observation equation is y(t)=Hs(t).  The prior distribution for
% s is N(shat,sig).  To handle the standard Kalman Filter setup where the observation
% equation is y(t)=Hs(t)+Nu(t), expand s to become [s;v] (where v=Nu), expand H to [H I], replace
% G by [G 0;0 0], and replace M with [M 0;0 N].  The resulting model has no "error" in the
% observation equation but is equivalent to the original model with error in the state equation.
% The posterior on the new state is N(shatnew,signew) and lh is a two-dimensional vector containing
% the increments to the two component terms of the log likelihood function.  They are added 
% to form the log likelihood, but are used separately in constructing a concentrated or marginal
% likelihood. yhat is the error in the forecast of y based on the prior.
lh=zeros(1,2);
if size(shat,2)>size(shat,1)
   shat=shat';
end
if size(y,2) > size(y,1)
   y=y';
end
omega=G * squeeze(sig) * G' + M*M';
[u0, d0, v0]=svd(omega);
[u d v]=svd(H*u0*sqrt(d0));
first0=min(find(diag(d)<1e-12));
if isempty(first0),first0=min(size(H))+1;end
u=u(:,1:first0-1);
v=v(:,1:first0-1);
d=diag(d);d=diag(d(1:first0-1));
fac=v0*sqrt(d0);
yhat = y- H * G * shat;
ferr=(v/d)*u'*yhat;
lh(1)=-.5*ferr'*ferr;
lh(2)=-sum(log(diag(d)));
shatnew=fac*ferr+G*shat;
signew=fac*(eye(size(v,1))-v*v')*fac';
