
function [U] = lyapSU(R,B);
%
%
%     Usage:  [U] = lyapSU(R,B);
%
%     Solves the Lyapunov equation
%
%         R P + P R' + B B' = 0
%
%     where R is real n by n quasi upper triangular matrix, 
%     and B is an m by n matrix.
%
%     Assumes eigenvalues of R are in open left half-plane.
%     Computes U upper triangular with positive diagonal elements
%     such that
%                P = U U'
%
%     D.C. Sorensen
%     sorensen@rice.edu
%     8 Jan 00
%
%--------------------------------------------------------------
%
    [n,n] = size(R);
%
%   Be sure R is fully deflated
%
    for j = 1:n-1,
        if (abs(R(j+1,j)) < 10*eps*max(abs(R(j,j)), abs(R(j+1,j+1)))),
            R(j+1,j) = 0;
        end
    end
%
    U = zeros(n);
%
%  Solve for U in the Schur basis.  
%
%  Uses structure  
%
%       inv(U)*R*U  is upper triangluar
%
    j = n;
    while(j > 0),
       if (j > 1), test = R(j,j-1); else test = 0; end
       if (test == 0),
          mu = R(j,j);
          b = B(1:j,:)*B(j,:)';
          U(1:j,j) = -(R(1:j,1:j) + mu*eye(j))\b;
          lam = U(j,j); 
          if(lam > 0), 
             slam = sqrt(lam);
             U(j,j) = slam;
             U(1:j-1,j) = U(1:j-1,j)/slam; 
             B(j,:) = B(j,:)/slam;
             B(1:j-1,:) = B(1:j-1,:) - U(1:j-1,j)*B(j,:);
          end
          j = j-1;
       else           
          [Y,D] = eig(R(j-1:j,j-1:j)');
          Y = [real(Y(:,1)) imag(Y(:,1))];
          mu = real(D(1,1)); eta = imag(D(1,1));
          b = B(1:j,:)*B(j-1:j,:)'; b = -b*Y;
          M = R(1:j,1:j) + mu*eye(j);
          b = M*b - eta*b*[0 1 ; -1 0];
          U(1:j,j-1:j) = (M*M + eta^2*eye(j))\b;
          U(1:j,j-1:j) = U(1:j,j-1:j)/Y;  
          U(j-1,j) = (U(j-1,j) + U(j,j-1))/2;
          U(j,j-1) = 0;
          lam = U(j,j); 
          if(lam > 0), 
             slam = sqrt(lam);
             U(j,j) = slam;
             U(1:j-1,j) = U(1:j-1,j)/slam; 
             U(1:j-1,j-1) = U(1:j-1,j-1) - U(1:j-1,j)*U(j-1,j); 
             B(j,:) = B(j,:)/slam;
             B(1:j-1,:) = B(1:j-1,:) - U(1:j-1,j)*B(j,:);
          end
          j = j-1;
          lam = U(j,j); 
          if(lam > 0), 
             slam = sqrt(lam);
             U(j,j) = slam;
             U(1:j-1,j) = U(1:j-1,j)/slam; 
             B(j,:) = B(j,:)/slam;
             B(1:j-1,:) = B(1:j-1,:) - U(1:j-1,j)*B(j,:);
          end
          j = j-1;
       end
    end

