function [K,S]=kfchan(A,C,Q,R,W);
%KFCHAN  Chandrasekhar algorithm for Kalman filter.
%        [K,S]=kfchan(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))']
%        (Note: if R not invertible the program calls KFVGHN.)
m=max(size(A));
[rc,cc]=size(C);
if rank(R)>=rc;
  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;


  s0=zeros(m);
  w0=R;
  k0=zeros(m,rc);
  e=ones(m,rc);
  m0=Q;
  y0=eye(m);
  dd=1;
  it=1;
  maxit=1000;
  tol=1e-8;                         % reset tol if necessary
  while (dd>tol & it<=maxit);
    ym=y0*m0*y0';
    w1=w0+C*ym*C';
    k1=(k0*w0+A*ym*C')/w1;
    y1=(A-k1*C)*y0;
    m1=(eye(m)+m0*y0'*(C'/w0)*C*y0)*m0;
    s1=s0+ym;
    dd=max(max(abs( (k1-k0)./max(abs(k0),e) )));
    it=it+1;
    w0=w1;
    k0=k1;
    y0=y1;
    m0=m1;
    s0=s1;
  end
  K=k1; S=s1;
  if any(any(W)); K=K+W/R; end;
  if it>=maxit; disp('WARNING: Iteration limit of 1000 in KFCHAN'); end;
else;
  [K,S]=kfvghn(A,C,Q,R,W);
end;
