function [f,p,V,index]=dlqrtes1(beta,A,B,Q,R,W);
[ns,nc]=size(B);
m1=[eye(ns) -B zeros(ns);zeros(nc,ns) R B';zeros(ns) beta*W beta*A'];
m2=[A zeros(ns,nc+ns);-W' zeros(nc,nc+ns);-beta*Q zeros(ns,nc) eye(ns)];
if rank(m1)>=2*ns+nc;
  [V,D]=eig(m1\m2);
  d=diag(D);
else;
  [V,D]=eig(m2,m1);
  d=diag(D);
  rdd=find(real(d)>=inf);
  if ~isempty(rdd);
    d(rdd)=real(d(rdd));
  end;
  rdd=find(real(d)==NaN);
  if ~isempty(rdd);
    d(rdd)=inf*ones(rdd);
  end;
end;
[d,index]=sort(abs(d));
p=V(nc+ns+1:2*ns+nc,index(1:nc+ns))/V(1:nc+ns,index(1:nc+ns));
p=p(:,1:ns);
f=real((R+B'*p*B)\(B'*p*A+W'));
p=real(p/beta);
