% [Preg,gbest,x1,x2,y1,y2]=plantreg(P,r,gama,x1,x2,y1,y2,sing12,sing21) % % Called by HINFRIC, NOT MEANT TO BE CALLED BY USERS. % % % Regularizes a plant P(s) where D12 or D21 are rank-deficient. % % The eps-regularization is optimized wrt. numerical stability % and magnitude of the controller parameters. GAMA is increased % to GBEST when necessary for numerical reliability. % % Input: % P plant state-space data in packed form % R size of D22 (R = [ # measurements , # controls ]) % GAMA required H-infinity performance % X1,X2,Y1,Y2 output of GOPTRIC % SING12 SING12 > 0 indicates that D12 is rank deficient % SING21 same for D21 % % Output % PREG regularized plant (in packed form) % GBEST best achievable performance in [GAMA, +Inf] after % regularization % X1,X2,Y1,Y2 X = X2/X1 and Y = Y2/Y1 are the stabilizing % solutions of the two H_infinity Riccati equations % for gamma = GBEST % % See also HINFRIC. % Authors: P. Gahinet and A.J. Laub 10/93 % Copyright 1995-2004 The MathWorks, Inc. % $Revision: 1.1.6.1 $ function [Preg,gopt,x1,x2,y1,y2]=... plantreg(P,r,gopt,x1,x2,y1,y2,sing12,sing21,knobrk,knobjw,macheps) if nargin<10, macheps=mach_eps; knobrk=0; knobjw=0; end % regular problem w/o gain limitation -> exit if knobrk < .1 & ~(sing12 | sing21), Preg=P; return end if sing12 | sing21, disp(sprintf('\n** regularizing D12 or D21 to compute K(s):')); else disp(sprintf('\n** improving the numerical conditioning of K(s):')); end % tolerances: % tolsing is the tolerance for singularity. The singular part of D12 % is determined by the requirement: || B2 D12^+ || < 1/tolsing tolsing=macheps^(3/8); toleig=macheps^(2/3); % set control parameters magobj=1e5/10^(2*knobrk); % target for norm of Ak,Bk,Ck maxmag=1e9/10^(2*knobrk); % target for max. mag. of Ak,Bk,Ck % retrieve the plant data [a,b1,b2,c1,c2,d11,d12,d21,d22]=hinfpar(P,r); na=size(a,1); [p1,m1]=size(d11); naX=na; m2X=size(d12,2); m1X=m1; naY=na; m2Y=size(d21,1); m1Y=p1; % scales of X/Y if isempty(x1), normX=0; else normX=norm(x2/x1,1); end if isempty(y1), normY=0; else normY=norm(y2/y1,1); end scaleX=max(1e-2,normX^(1/3)); scaleY=max(1e-2,normY^(1/3)); % set-up for regularization loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% aX=a; b1X=b1; c1X=c1; b2X=b2; d11X=d11; Z12=1; Z12p=[]; aY=a'; b1Y=c1'; c1Y=b1'; b2Y=c2'; d11Y=d11'; Z21=1; Z21p=[]; [u,s12,v12]=svd(d12); nor12=s12(1,1); nb2X=norm(b2X,1); rk=length(find(s12 > max(toleig*nor12,macheps*nb2X))); s12=u*s12; b2X=b2X*v12; d12X=s12; absX=[abs(b2X);nor12*ones(1,m2X)]; tolX=toleig*nb2X*ones(1,m2X); % remove jw-axis zeros aX=jwreg(aX,b2X(:,1:rk),c1X,s12(:,1:rk),knobjw); p1Xjw=size(d11X,1); [u,s21,v21]=svd(d21'); nor21=s21(1,1); nb2Y=norm(b2Y,1); rk=length(find(s21 > max(toleig*nor21,macheps*nb2Y))); s21=u*s21; b2Y=b2Y*v21; d12Y=s21; absY=[abs(b2Y);nor21*ones(1,m2Y)]; tolY=toleig*nb2Y*ones(1,m2Y); % remove jw-axis zeros aY=jwreg(aY,b2Y(:,1:rk),c1Y,s21(:,1:rk),knobjw); p1Yjw=size(d11Y,1); ZZ=Z12'*Z21; % Hamiltonian set-up HX11=mdiag(aX,-aX'); HX33=mdiag(-eye(m1X),zeros(m2X)); HY11=mdiag(aY,-aY'); HY33=mdiag(-eye(m1Y),zeros(m2Y)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % REGULARIZATION LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Initialization stopX=0; stopY=0; % termination flags accelX=1; accelY=1; % acceleration flags regX=tolsing; regY=tolsing; % initial regularization thresholds g=gopt; % initial gamma compX=(sing12 > 0); compY=(sing21 > 0); betaX=regX; betaY=regY; magX=1e99; magY=1e99; % initialize magnitude estimates if ~sing12, if normX>100, sclf=sqrt(normX); tmp=orth([x1*sclf;x2/sclf]); else sclf=1; tmp=[x1;x2]; end magX=norm(tmp'*[c1X'*d12X;b2X*sclf^2]/(d12X'*d12X)*b2X',1); end if ~sing21, if normY>100, sclf=sqrt(normY); tmp=orth([y1*sclf;y2/sclf]); else sclf=1; tmp=[y1;y2]; end magY=norm(tmp'*[c1Y'*d12Y;b2Y*sclf^2]/(d12Y'*d12Y)*b2Y',1); end fprintf(1,' '); while ~(stopX & stopY), %%%%%%%%%%%%%%%%%%%%%%% pass=1; fprintf(1,'.'); % regularize D12 for current betaX as % ( C1X ) ( D11X ) ( D12X ) % C1X -> ( 0 ) D11X -> ( 0 ) D12X -> ( reg. ) % same for D21 scale=max([betaX*absX;tolX]); ratio=max([s12;zeros(1,m2X)])./scale; ind2X=find(ratio < 1); d12X=s12; stmpX=scale(ind2X); l2X=length(ind2X); p1X=p1Xjw+l2X; if l2X > 0, d12X(p1Xjw+(1:l2X),ind2X)=diag(stmpX); z=orth(d12X); z=eye(p1X)-z*z'; absmin=1.01*norm(z(:,1:p1Xjw)*d11X); if absmin > g, % reset gopt to lower bound absmin if appropriate if compX < 2, % betaX = regX -> reset gopt gopt=absmin; g=gopt; compY=1; elseif magX > maxmag/100, % still large mag. -> reset g g=absmin; compY=1; else % undo last increase of betaX pass=0; stopX=1; end end elseif compX==2, compX=0; end scale=max([betaY*absY;tolY]); ratio=max([s21;zeros(1,m2Y)])./scale; ind2Y=find(ratio < 1); d12Y=s21; stmpY=scale(ind2Y); l2Y=length(ind2Y); p1Y=p1Yjw+l2Y; if l2Y > 0, d12Y(p1Yjw+(1:l2Y),ind2Y)=diag(stmpY); z=orth(d12Y); z=eye(p1Y)-z*z'; absmin=1.01*norm(z(:,1:p1Yjw)*d11Y); if absmin > g, if compY < 2, % betaY = regY -> reset gopt gopt=absmin; g=gopt; compX=1; elseif magY > maxmag/100, % still large mag. -> reset g g=absmin; compX=1; else % undo last increase of betaY pass=0; stopY=1; end end elseif compY==2, compY=0; end if pass & compX, % recompute X c1Xt=[c1X ; zeros(l2X,naX)]; d11Xt=[d11X ; zeros(l2X,m1X)]; scalbd=diag(max(abs([b2X;d12X]))); b2Xt=b2X/scalbd; d12Xt=d12X/scalbd; HX12=[zeros(naX,p1X);-c1Xt'] ; HX13=[b1X/g b2Xt;zeros(naX,m1X+m2X)]; HX21=[c1Xt zeros(p1X,naX)] ; HX31=[zeros(m1X,naX) b1X'/g;zeros(m2X,naX) b2Xt']; HX23=[d11Xt/g d12Xt]; HX=[HX11 HX12/scaleX scaleX*HX13;HX21/scaleX -eye(p1X) HX23;scaleX*HX31 HX23' HX33]; [x1t,x2t]=ricpen(HX,mdiag(eye(2*naX),zeros(p1X+m1X+m2X))); pass=(naX == 0 | ~isempty(x1t)); if naX > 0 & pass, [d1,d2]=qz(x1t,x2t,'complex'); d1=diag(d1); d2=diag(d2); pass=(min(abs(d1)) > macheps*scaleX^2); if pass, pass=(min(real(d2./d1)) > -toleig); end x2t=x2t*scaleX^2; sqnorX=sqrt(max(abs(d2./d1)))*scaleX; end else x1t=x1; x2t=x2; end if pass & compY, % recompute Y c1Yt=[c1Y ; zeros(l2Y,naY)]; d11Yt=[d11Y ; zeros(l2Y,m1Y)]; scalbd=diag(max(abs([b2Y;d12Y]))); b2Yt=b2Y/scalbd; d12Yt=d12Y/scalbd; HY12=[zeros(naY,p1Y);-c1Yt']; HY13=[b1Y/g b2Yt;zeros(naY,m1Y+m2Y)]; HY21=[c1Yt zeros(p1Y,naY)]; HY31=[zeros(m1Y,naY) b1Y'/g;zeros(m2Y,naY) b2Yt']; HY23=[d11Yt/g d12Yt]; HY=[HY11 HY12/scaleY HY13*scaleY;HY21/scaleY -eye(p1Y) HY23;HY31*scaleY HY23' HY33]; [y1t,y2t]=ricpen(HY,mdiag(eye(2*naY),zeros(m1Y+p1Y+m2Y))); pass=(naY == 0 | ~isempty(y1t)); if naY > 0 & pass, [d1,d2]=qz(y1t,y2t,'complex'); d1=diag(d1); d2=diag(d2); pass=(min(abs(d1)) > macheps*scaleY^2); if pass, pass=(min(real(d2./d1)) > -toleig); end y2t=y2t*scaleY^2; sqnorY=sqrt(max(abs(d2./d1)))*scaleY; end else y1t=y1; y2t=y2; end rho=~pass*gopt; if naX > 0 & naY > 0 & pass & (compX | compY), xx1=mdiag(x1t,eye(na-naX)); xx2=mdiag(x2t,zeros(na-naX)); yy1=mdiag(y1t,eye(na-naY)); yy2=mdiag(y2t,zeros(na-naY)); rho=max(real(eig(xx2'*ZZ*yy2,xx1'*ZZ*yy1))); if rho > 0, rho=sqrt(rho); pass=(rho < (1+toleig)*g); end end % estimate the corresponding norm of BK and CK % NB: rescaling of x1t,x2t is consistent with KRIC if naX > 0 & compX & pass, if sqnorX>10, sclf=sqnorX; else sclf=1; end tmp=orth([x1t*sclf;x2t/sclf]); magX=norm(tmp'*[c1X'*s12;(b2X+b1X*d11X'*s12/g^2)*sclf^2]/(d12X'*d12X)*b2X',1); end if naY > 0 & compY & pass, if sqnorY>10, sclf=sqnorY; else sclf=1; end tmp=orth([y1t*sclf;y2t/sclf]); magY=norm(tmp'*[c1Y'*s21;(b2Y+b1Y*d11Y'*s21/g^2)*sclf^2]/(d12Y'*d12Y)*b2Y',1); end %%%%%%%%%%%%% decision making %%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%% --------------- %%%%%%%%%%%%%%%%%%%%%%% if pass, % set gopt to current g, increase betaX, betaY gopt=g; regX=betaX; regY=betaY; if compX, x1=x1t; x2=x2t; scaleX=min(1e5,max(1e-3,sqnorX)); end if compY, y1=y1t; y2=y2t; scaleY=min(1e5,max(1e-3,sqnorY)); end stopX=stopX | magX < magobj | (magX <= maxmag & betaX > 1); stopY=stopY | magY < magobj | (magY <= maxmag & betaY > 1); compX=2*(~stopX & (magX > magY | stopY)); compY=2*(~stopY & (magY >= magX | stopX)); elseif betaX > 1e3*regX | betaY > 1e3*regY, % turn off acceleration accelX=accelX & (betaX <= 100*regX); betaX=regX; accelY=accelY & (betaY <= 100*regY); betaY=regY; else % increase gamma, or stop increasing betaX, betaY betaX=regX; betaY=regY; if max(magX,magY) > maxmag & gopt inc. gamma fact=1.3+.4*(gopt > 1e3)+2*(gopt < 1e-2); gopt=gopt*fact; if rho > 0, gopt=max(gopt,sqrt(rho*gopt)); end if gopt>1e6, gopt=Inf; end g=gopt; compX=1; compY=1; else % can no longer increase betaX, betaY stopX=stopX | (compX == 2); compX=2*(~stopX); stopY=stopY | (compY == 2); compY=2*(~stopY); end end %% update beta if compX==2, betaX=max(100,accelX*magX/1e6)*betaX; end if compY==2, betaY=max(100,accelY*magY/1e6)*betaY; end end %%% WHILE fprintf(1,'\n\n'); %%%%%%%%%%%%% FORM THE REGULARIZED PLANT %%%%%%%%%%%%%%%%%%%%%%% scale=max([regX*absX;tolX]); ratio=max([s12;zeros(1,m2X)])./scale; ind2=find(ratio < 1); l2=length(ind2); if l2>0, stmp=zeros(l2,m2X); stmp(:,ind2)=diag(scale(ind2)); d12=[d12;stmp*v12']; d11=[d11;zeros(l2,m1)]; c1=[c1;zeros(l2,na)]; p1=size(d11,1); end scale=max([regY*absY;tolY]); ratio=max([s21;zeros(1,m2Y)])./scale; ind2=find(ratio < 1); l2=length(ind2); if l2>0, stmp=zeros(m2Y,l2); stmp(ind2,:)=diag(scale(ind2)); d21=[d21 v21*stmp]; d11=[d11 zeros(p1,l2)]; b1=[b1 zeros(na,l2)]; end Preg=ltisys(a,[b1 b2],[c1;c2],[d11 d12;d21 d22]); %CHECK1(a,b1,c1,b2,d12,d11,x1,x2,gopt); %CHECK1(a',c1',b1',c2',d21',d11',y1,y2,gopt);