function [f1,p] = dricc(beta,A,B,Q,R,W)
%DRICC calculates f of the feedback law:  u = -fx  that maximizes 
%
%          sum {beta^t [x'Qx + u'Ru +2x'Wu] }
%       
%  subject to 
%		x[t+1] = Ax[t] + Bu[t] 
%
%  where x is the mx1 vector of states, u is the kx1 vector of controls,
%  A is mxm, B is mxk, Q is mxm, R is kxk, W is mxk.
%                
%  INPUTS: beta, A, B, Q, R, W  (or just beta, A, B, Q, R if there are no
%          cross products, W=0).  Set beta=1, if there is no discounting.
%
%  OUTPUTS: 
%    (i) f= (R+beta*B'*p*B)\(beta*B'*p*A+W'), the decision rule.
%   (ii) p= beta*A'*p*A + Q -(beta*A'*p*B+W)*f, the solution to the matrix
%           Riccati equation
%
%  NOTES: see also OLRP, DOUBLE
%
m=max(size(A));
[rb,cb]=size(B);
if nargin==5; W=zeros(m,cb); end;
p=-eye(m);
f0=(R+beta*B'*p*B)\(beta*B'*p*A+W');
dd=1;
it=1;
maxit=5000;
tol=1e-8;       % reset tolerance if necessary 
while (dd> tol & it<= maxit);
  p =beta*A'*p*A + Q -(beta*A'*p*B+W)*f0;
  f1=(R+beta*B'*p*B)\(beta*B'*p*A+W');
  dd=max(max(abs( (f1-f0)./(1+abs(f0)) )));
dd
  it=it+1;
  f0=f1;
end;
if it>=maxit; disp('WARNING: Iteration limit of 5000 reached in DRICC'); end;
