function [f,p] = olrp(beta,A,B,Q,R,W)
%OLRRIC Calculation of feedback laws for linear-quadratic optimization.
%       [f,p]=olrricc(beta,A,B,Q,R,W) calculates the optimal feedback law,
%       f, such that u=-fx  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.
%                
%       Also returned is p where
% 
%        f= (R+beta*B'*p*B)\(beta*B'*p*A+W'), the decision rule, depends on p
%        p= beta*A'*p*A + Q -(beta*A'*p*B+W)*f, is the solution to the 
%       matrix Riccati equation.
%
%       [f,p]=olrp(beta,A,B,Q,R) can be used if there are no cross-products. 
%

%       Ellen R. McGrattan,  10-13-87
%       Revised 11-23-88,  ERM
%
m=max(size(A));
[rb,cb]=size(B);
if nargin==5; W=zeros(m,cb); end;
p=-.01*eye(m);
f0=(R+beta*B'*p*B)\(beta*B'*p*A+W');
dd=1;
it=1;
maxit=1000;
tol=1e-8;                           % reset tol if necessary
while (dd>tol & it<=maxit);
  p =beta*A'*p*(A-B*f0) + Q -W*f0;
  f1=(R+beta*B'*p*B)\(beta*B'*p*A+W');
  dd=max(max(abs( (f1-f0)./(1+abs(f0)) )));
  it=it+1;
  f0=f1;
end;
f=f1;
if it>=maxit; 
  disp('WARNING: Iteration limit of maxit reached in OLRP',maxit) 
end;
