www.gusucode.com > rctobsolete 工具箱 matlab源码程序 > rctobsolete/lmi/ricpen.m
% [X1,X2] = ricpen(H,L) % X = ricpen(H,L) % % Generalized Schur Solver for continuous-time Riccati equations. % % Computes the stabilizing solution X = X2/X1 of the % Riccati equation associated with the Hamiltonian pencil % % [ A F S1 ] [ E 0 0 ] % H - t L = [ G -A' -S2 ] - t [ 0 E' 0 ] % [ S2' S1' R ] [ 0 0 0 ] % % The blocks S1, S2, R may be all empty. % % Assumptions: % * R and E are invertible, % * the pencil H - t L has no finite generalized eigenvalue on % the imaginary axis. % % When L is omitted, RICPEN solves the Riccati equation of Hamiltonian % matrix H (L and R are set to the default value L=I and R=[]). % If H - t L has imaginary-axis eigenvalues, X, X1, and X2 are % empty on output. In addition, X is empty whenever X1 is singular. % % % See also CARE. % Authors: A.J. Laub and P. Gahinet 10/93 % Copyright 1995-2004 The MathWorks, Inc. function [x1,x2,d1,d2]=ricpen(h,le) x1=[]; x2=[]; d1=[]; d2=[]; if size(h,1)~=size(h,2), error('H must be a square matrix'); elseif nargin == 1, le=eye(size(h)); elseif nargin>2, error(sprintf(... ['usage: X = ricpen(H,L) or [X1,X2] = ricpen(H,L)'])); end % determine size of E if norm(le,1)==0, return, end m=0; i=size(h,1); while max(abs(le(:,i)))==0, m=m+1; i=i-1; end n2=size(h,1)-m; if 2*round(n2/2)~=n2, error('[ E 0 ; 0 E'' ] must have even row size'); end n=n2/2; % number of states e=le(1:n,1:n); macheps=mach_eps; toleig=macheps^(3/4); % relative tolerance for imag. axis eigenvalues % compression if E is singular if m>0, h2=h(:,n2+1:n2+m); h22=h2(n2+1:n2+m,:); % test whether R is invertible s=rcond(h22); if s < macheps, disp('RICPEN: R is singular to the machine precision');return elseif s > sqrt(mach_eps), h=h(1:n2,1:n2)-h(1:n2,n2+1:n2+m)/h22*h(n2+1:n2+m,1:n2); le=le(1:n2,1:n2); else [q,r] = qr(h2); h = q(:,m+1:m+n2)'*h(:,1:n2); le = q(:,m+1:m+n2)'*le(:,1:n2); end end % Do initial QZ algorithm; eigenvalues of the pencil E - t H % will have a tendency to deflate out in the "desired" order [aa,bb,q,z] = qz(h,le,'complex'); % Order eigenvalues; this part of the code is adapted from % Kjell Gustafsson's genric m-file and will be replaced with % a new real re-ordering code that works with a QZ algorithm % implemented in real arithmetic; it may also be replaced % with a simpler and more efficient complex code % Find all eigenvalues outside the open left-half plane daa = diag(aa); dbb = diag(bb); ind=real(daa./dbb) < -toleig; if sum(ind) ~= n, return, end % This part of the code is a direct translation of the sorting % of 1x1 eigenvalue blocks in ACM TOMS Algorithm 590 j = find(ind(1:n2)==0); k = find(ind(j(1)+1:n2)==1); while ~isempty(k) j = j(1); k = k(1)+j; % Propagate element up the diagonal from k-th to j-th entry for l = k-1:-1:j l1 = l+1; % exchange elements l and l1 using EXCHQZ from Algorithm 590 f = max(abs([aa(l1,l1),bb(l1,l1)])); altb = abs(aa(l1,l1)) < f; % construct the column transformation zt zt = givens((bb(l,l)*aa(l1,l1)-aa(l,l)*bb(l1,l1))/f, ... (bb(l,l1)*aa(l1,l1)-aa(l,l1)*bb(l1,l1))/f); zt = [zt(2,:); zt(1,:)]; aa(:,l:l1) = aa(:,l:l1)*zt; bb(:,l:l1) = bb(:,l:l1)*zt; z(:,l:l1) = z(:,l:l1)*zt; % construct the row transformation qt if altb qt = givens(bb(l,l),bb(l1,l)); else qt = givens(aa(l,l),aa(l1,l)); end aa(l:l1,:) = qt*aa(l:l1,:); bb(l:l1,:) = qt*bb(l:l1,:); ix = ind(l); ind(l) = ind(l1); ind(l1) = ix; end; % Find next element to shift j = find(ind(1:n2)==0); k = find(ind(j(1)+1:n2)==1); end; if nargin > 1, [q,r] = qr([e*z(1:n,1:n);z(n+(1:n),1:n)]); x1=q(1:n,1:n); x2=q(n+(1:n),1:n); else x1=z(1:n,1:n); x2=z(n+(1:n),1:n); end % check that X1'*X2 is symmetric x12=x1'*x2; if norm(x12-x12',1) > macheps^(1/4), x1=[], x2=[]; return end % Solve for X or make X1,X2 real via ``QZ symmetrization trick'' [d1,d2,u,v] = qz(x1,x2,'complex'); d1=diag(d1);d2=diag(d2); if nargout==1, if min(abs(d1)) < macheps, x1=[]; else x1 = real(u'*diag(d2./d1)*u); end else % make X1,X2 real rot=zeros(size(d1)); ix1=find(abs(d1)>=abs(d2)); rot(ix1)=conj(d1(ix1))./abs(d1(ix1)); ix2=find(abs(d1)<abs(d2)); rot(ix2)=conj(d2(ix2))./abs(d2(ix2)); x1=real(u'*diag(d1.*rot)*u); x2=real(u'*diag(d2.*rot)*u); end % *** last line of ricpen.m ***