function [f,p]=dlqrsv(beta,A,B,Q,R,W)
%OLRP can have arguments: (beta,A,B,Q,R) if there are no cross products
%     (i.e. W=0).  Set beta=1, if there is no discounting.
%
%     OLRP calculates f of the feedback law:
%               
%		u = -fx
%  
%  that maximizes the function:
%
%          sum {beta^t [x'Qx + u'Ru +2x'Wu] }
%  
%  subject to 
%		x[t+1] = Ax[t] + Bu[t] 
%
%  where x is the nx1 vector of states, u is the kx1 vector of controls,
%  A is nxn, B is nxk, Q is nxn, R is kxk, W is nxk.
%                
%    Also returned is p, the steady-state solution to the associated 
%  discrete matrix Riccati equation.
%
m=max(size(A));
[rb,cb]=size(B);
if nargin==5; W=zeros(m,cb); end;
if rank(R)>=cb;
  A=sqrt(beta)*(A-B*(R\W'));
  B=sqrt(beta)*B;
  Q=Q-W*(R\W');
  M=[A zeros(m);-Q eye(m)];
  G=B*(R\B');
  L=[eye(m) G;zeros(m) A'];
  [MM,LL,QQ,Z]=qz(M,L);
  [V,D]=eig(M,L);
  d=(diag(D)>ones(2*m,1));
  if sum(d)~=m; error('wrong number of stable eigenvalues'); end;
  done=0;
  while ~done;
    switch=0;
    it=1;
    while ~switch & it<=2*m-1;
      it1=it+1;
      if d(it)>d(it1);
        tpos=d(it);
        d(it)=d(it1);
        d(it1)=tpos;
        adbc=MM(it,it)*LL(it1,it1)-MM(it1,it1)*LL(it,it);
        S=(LL(it,it1)*MM(it1,it1)-MM(it,it1)*LL(it1,it1))/adbc;
        T=(MM(it,it1)*LL(it,it)-LL(it,it1)*MM(it,it))/adbc;
        S1=1/sqrt(1+S*S);
        T1=1/sqrt(1+T*T);
        Q2=[T*T1 -T1; T1 T*T1];
        Z2=[S*S1  S1; S1 -S*S1];
        tmatq=eye(2*m);
        tmatz=tmatq;
        tmatq(it:it1,it:it1)=Q2;
        tmatz(it:it1,it:it1)=Z2;
        Q2=tmatq;
        Z2=tmatz;
        MM=Q2*MM*Z2;
        LL=Q2*LL*Z2;
        Z=Z*Z2;
        switch=1;
      end;
      it=it1;
    end;
    done=~max(d(1:m));
  end;
  U=Z(:,1:m);
  p=U(m+1:2*m,:)/U(1:m,:);
  f=(R+B'*p*B)\(B'*p*A)+R\W';
else;
  p0=-.01*eye(m);
  dd=1;
  it=1;
  maxit=5000;
  % check tolerance; for greater accuracy set it to 1e-10
  while (dd>1e-10 & it<=maxit);
    f0=   (R+beta*B'*p0*B)\(beta*B'*p0*A+W')
    p1=beta*A'*p0*A + Q -(beta*A'*p0*B+W)*f0;
    f1=   (R+beta*B'*p1*B)\(beta*B'*p1*A+W');
    dd=max(max(abs((f1-f0)./max(abs(f0),ones(f0)))));
    it=it+1;
    p0=p1;
  end;
  f=f1;p=p0;
  if it>=maxit; disp('WARNING: Iteration limit of 5000 reached in OLRP'); end;
end;
