function [K,S]=kfvghn(A,C,Q,R,W);
%KFVGHN  Vaughan's algorithm for Kalman filter.
%        [K,S]=kfvghn(A,C,Q,R,W) calculates the Kalman gain, K, and
%        the stationary covariance matrix, S, obtained from the 
%        Kalman filter with the following system:
%
%	  x[t+1] = Ax[t] + Bu[t] + w1[t+1]
%           y[t] = Cx[t] + Du[t] + w2[t]
%                                                          (1)
%         E [w1(t+1)] [w1(t+1)]' =  [Q  W;
%           [ w2(t) ] [ w2(t) ]      W' R]
%
%        where x is the mx1 vector of states, u is the nx1 vector of 
%        controls, y is the px1 vector of observables, A is mxm, B is
%        mxn, C is pxm, Q is mxm, R is pxp, and W is mxp.  The 
%        innovations representation of system (1) is then
%         _               _                     _
%         x[t+1] = (A-K*C)x[t] + Bu[t] +Ky[t],  x[t]=E[ x[t] | y[t],y[t-1]..]
%                         _   
%           y[t] = Cx[t] +Du[t] +a[t]                            
%                                                       _            _
%        where K = (A*S*C'+W)/(R+C*S*C'),  S= E[(x(t)-x(t))*(x(t)-x(t))']

%        Ellen R. McGrattan, 6-23-89
%        
B=C'; A=A';
[ns,nc]=size(B);
m1=[eye(ns) -B zeros(ns);zeros(nc,ns) R B';zeros(ns) W A'];
m2=[A zeros(ns,nc+ns);-W' zeros(nc,nc+ns);-Q zeros(ns,nc) eye(ns)];
if rank(m1)>=2*ns+nc;
  [V,D]=eig(m1\m2);
  d=diag(D);
else;
  [V,D]=eig(m2,m1);
  d=diag(D);
  rdd=find(real(d)>=inf);
  if ~isempty(rdd);
    d(rdd)=real(d(rdd));
  end;
end;
[d,index]=sort(abs(d));
p=V(nc+ns+1:2*ns+nc,index(1:nc+ns))/V(1:nc+ns,index(1:nc+ns));

S=real(p(:,1:ns));
K=(R+B'*S*B)\(B'*S*A+W');
