function [K,S]=kfdble(A,C,Q,R,W);
%KFDBLE  Doubling algorithm for Kalman filter.
%        [K,S]=kfdble(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
%        

if rank(R)>=length(R);
  if any(any(W));
    A=A-W*(R\C);
    Q=Q-W*(R\W');
  end;
  m=max(size(A));
  v=eye(m);
  a0=A';
  b=C'*(R\C);
  g=Q;
  k0=A*g*C'/(C*g*C'+R);
  dd=1;
  it=1;
  maxit=1000;
  tol=1e-8;                         % reset tol if necessary
  while (dd>tol & it<=maxit);
    v1=(v+b*g);
    vv=v1\a0; 
    a1=a0*vv;
    b=b+a0*(v1\(b*a0'));
    g=g+a0'*g*vv;
    k1=A*g*C'/(C*g*C'+R);
    dd=max(max(abs( (k1-k0)./(1+abs(k0)) )));
    it=it+1;
    a0=a1;
    k0=k1;
  end
  K=k1; S=g;
  if it>=maxit; 
    fprintf('WARNING: Iteration limit of %g reached in DOUBLE',maxit)
  end;
  if any(any(W)); K=K+W/R; end;
else;
  [K,S]=kfricc(A,C,Q,R,W);
end;
