% [K,gfin]=dkcen(P,r,gama,X1,X2,Y1,Y2,tolred) % % Given a discrete-time plant P and GAMA > 0, DKCEN % computes a "central" H-infinity digital controller K(s) that % * internally stabilizes the plant, % * yields a closed-loop gain no larger than GAMA. % This controller is calculated using either the stabilizing % solutions X,Y of the two H_infinity Riccati equations % (classical approach), or any solutions X,Y of the Riccati % inequalities (LMI approach). % % Input: % P plant SYSTEM matrix (see LTISYS) % R size of D22, i.e., R = [ NY , NU ] where % NY = number of measurements % NU = number of controls. % GAMA required H-infinity performance % X1,X2,.. Riccati approach: X = X2/X1 and Y = Y2/Y1 % are the stabilizing solutions of the two % Riccati equations % LMI approach: X = X2/X1 and Y = Y2/Y1 are % solutions of the two H_infinity Riccati % inequalities. % TOLRED optional (default value = 1.0e-4). Reduced-order % synthesis is performed whenever % rho(X*Y) >= (1 - TOLRED) * GAMA^2 % Output: -1 % K Central controller K(s) = DK + CK (zI-AK) BK in % packed form. % GFIN guaranteed closed-loop performance (GAMA may be reset % to GFIN > GAMA for numerical stability reasons) % % % See also DHINFRIC. % Authors: P. Gahinet and A.J. Laub 10/93 % Copyright 1995-2004 The MathWorks, Inc. % $Revision: 1.1.8.1 $ function [Kcen,gfin]=dkcen(P,r,gama,x1,x2,y1,y2,tolred) if nargin<7, error('usage: K = dkcen(P,r,gama,x1,x2,y1,y2,tolred)'); elseif nargin<8, tolred=1.0e-4; end % tolerances macheps=mach_eps; tolsing=sqrt(macheps); toleig=macheps^(2/3); % retrieve the plant data [a,b1,b2,c1,c2,d11,d12,d21,d22]=hinfpar(P,r); na=size(a,1); [p1,m1]=size(d11); [p2,m2]=size(d22); % rescale the plant data to keep the conditioning of M,N small sclf=1; sclx=sqrt(norm(x2/x1,1)); if sclx > 10, c1=c1/sclx; d11=d11/sclx; d12=d12/sclx; gama=gama/sclx; x1=x1*sclx; x2=x2/sclx; sclf=sclf*sclx; end scly=sqrt(norm(y2/y1,1)); if scly > 10, b1=b1/scly; d11=d11/scly; d21=d21/scly; gama=gama/scly; y1=y1*scly; y2=y2/scly; sclf=sclf*scly; end % orthogonalize [x1;x2] and [y1;y2] (systematically because % of the subsequent diagonalization of x1,x2) [q,rr]=qr([x1;x2]); x1=q(1:na,1:na); x2=q(na+1:2*na,1:na); [q,rr]=qr([y1;y2]); y1=q(1:na,1:na); y2=q(na+1:2*na,1:na); % diagonalize x1,x2. Since X>=0, u1'*x2*v1 is diagonal [ux1,sx1,vx1]=svd(x1); sx2=real(diag(ux1'*x2*vx1)); [uy1,sy1,vy1]=svd(y1); sy2=real(diag(uy1'*y2*vy1)); % zero small entries ind=find(sx2 < toleig); sx2(ind)=zeros(size(ind)); sqsx2=diag(sqrt(sx2)); ind=find(sy2 < toleig); sy2(ind)=zeros(size(ind)); sqsy2=diag(sqrt(sy2)); % SVD's of [b2;d12] and [c2 d21] %------------------------------- BD=[b2'*ux1*sqsx2,d12']; [uC,sC,vC,rC]=svdparts(BD,toleig*norm(b2,1),tolsing); CD=[c2*uy1*sqsy2,d21]; [uB,sB,vB,rB]=svdparts(CD,toleig*norm(c2,1),tolsing); % compute a solution of the characteristic LMI for DK % --------------------------------------------------- aux=[sqsx2*ux1'*a*uy1*sqsy2 , sqsx2*ux1'*b1; c1*uy1*sqsy2 d11]; MD=[-gama*mdiag(sx1,eye(p1)) aux;aux' , -gama*mdiag(sy1,eye(m1))]; PD=[BD,zeros(m2,na+m1)]; QD=[zeros(p2,na+p1),CD]; eigMD=eig(MD); if max(real(eigMD)) < toleig*max(abs(eigMD)), dk=zeros(m2,p2); else dk=basiclmi(MD,PD,QD,'Xmin,Shift'); end AA=a+b2*dk*c2; BB=b1+b2*dk*d21; CC=c1+d12*dk*c2; Dcl=d11+d12*dk*d21; % compute the inverse square root of DELcl aux=PD'*dk*QD; DELcl=-(MD+aux+aux')/gama; [u,t]=schur(DELcl); t=real(diag(t)); ind=find(t > tolsing); t=sqrt(1./t(ind)); u=u(:,ind); DELsqi=diag(t)*u'; % sqrt(inv(DELcl)) %disp(sprintf('min eval of DELcl: %6.3e',min(real(eig(DELcl))))); % detect full vs reduced %----------------------- x2y2=x2'*y2; x1y1=x1'*y1; gev=eig(x2y2,x1y1); gama=max(gama,sqrt(max(0,max(real(gev))))); gama0=gama; % svd of x2'y2/gama^2-x1'y1 -> M,N [vm,srs,vn]=svd(x2y2/gama^2-x1y1); shalf=sqrt(diag(srs)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % SYNTHESIS STARTS % BOTH FULL- AND REDUCED-ORDER CASES ARE TREATED AT ONCE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ContinueLoop=1; while ContinueLoop, rkred=length(find(gev < (1-tolred)*gama^2)); vmr=vm(:,1:rkred); vnr=vn(:,1:rkred); shalfr=shalf(1:rkred); y1vn=y1*vnr; x1vm=x1*vmr; % compute B_K %------------- aux1=[-sqsx2*ux1'*y2/gama;zeros(p1,na)]*vnr; aux2=[sqsy2*uy1'*a'*y1vn;b1'*y1vn]; if rB==0, thetaB=zeros(p2,rkred); else tmp=vB*diag(sB); thetaB=-[zeros(rB) zeros(rB,na+p1) tmp';[zeros(na+p1,rB); tmp] -DELcl]\... [zeros(rB,rkred);aux1;aux2]; thetaB=uB*thetaB(1:rB,:); end auxB=DELsqi*[aux1;aux2+[sqsy2*uy1'*c2';d21']*thetaB]; %DS=-vnr'*y1'*y2*vnr+auxB'*auxB; %disp(sprintf('max evals of DS: %6.3e',max(real(eig(DS))))); % compute C_K %------------ aux1=[sqsx2*ux1'*a*x1vm;c1*x1vm]; aux2=[-sqsy2*uy1'*x2/gama;zeros(m1,na)]*vmr; if rC==0, thetaC=zeros(m2,rkred); else tmp=vC*diag(sC); thetaC=-[zeros(rC) tmp' zeros(rC,na+m1);[tmp; zeros(na+m1,rC)] -DELcl]\... [zeros(rC,rkred);aux1;aux2]; thetaC=uC*thetaC(1:rC,:); end auxC=DELsqi*[aux1+[sqsx2*ux1'*b2;d12]*thetaC;aux2]; %DR=-vmr'*x1'*x2*vmr+auxC'*auxC; %disp(sprintf('max evals of DR: %6.3e',max(real(eig(DR))))); vny1=y1vn'; bk=diag(1./shalfr)*(thetaB'-vny1*b2*dk); ck=(thetaC-dk*c2*x1vm)*diag(1./shalfr); ak=-diag(1./shalfr)*(vny1*(a-b2*dk*c2)*x1vm+thetaB'*c2*x1vm+... vny1*b2*thetaC+auxB'*auxC/gama)*diag(1./shalfr); % validation in the case of reduced-order synthesis ContinueLoop=0; if rkred < na, if max(abs(eig([AA b2*ck;bk*c2 ak]))) >= 1, %disp('REDUCED FAILED'); tolred=tolred/100; ContinueLoop=1; if tolred < toleig, rkred=na; else rkred=max(rkred+1,length(find(gev < (1-tolred)*gama0^2))); end end end end % while ContinueLoop Kcen=ltisys(ak,bk,ck,dk); gfin=gama*sclf; %%%%%%%%%%%%%%%% TMP %%%%%%%%%%%% % follows formulas in kcen.tex % %M=vm*diag(shalf); N=vn*diag(shalf); % %rk=size(M,2); %Pcl=[x1 y2/gama;M' zeros(rk,na)]; %Qcl=[x2/gama y1; zeros(rk,na) N']; %Acl=[AA b2*ck;bk*c2 ak]; %Bcl=[BB;bk*d21]; %Ccl=[CC d12*ck]; % %BRL=gama*[-Qcl'*Pcl Qcl'*Acl*Pcl Qcl'*Bcl zeros(2*na,p1);... % Pcl'*Acl'*Qcl -Qcl'*Pcl zeros(2*na,m1) Pcl'*Ccl';... % Bcl'*Qcl zeros(m1,2*na) -gama*eye(m1) Dcl';... % zeros(p1,na*2) Ccl*Pcl Dcl -gama*eye(p1)]; %D=mdiag(eye(4*na),eye(m1+p1)/gama); %BRL=D*BRL*D; % %perm=[2*na+(1:na),na+(1:na),1:na,4*na+m1+(1:p1),3*na+(1:na),4*na+(1:m1)]; %BRL=BRL(perm,perm); %D=mdiag(eye(2*na),mdiag(mdiag(vx1,eye(p1)),mdiag(vy1,eye(m1)))); %BRL=D'*BRL*D; % %DDEL=BRL(2*na+1:4*na+m1+p1,2*na+1:4*na+m1+p1); %DD=mdiag(mdiag(sqsx2,eye(p1)),mdiag(sqsy2,eye(m1))); %gap_DELcl=norm(DDEL+DD*DELcl*DD) % %BRLr=schrcomp(BRL,2*na+1,4*na+m1+p1,macheps); %maxeigBRL=max(real(eig(BRLr))) % %DRR=BRLr(1:na,1:na); %DSS=BRLr(na+(1:na),na+(1:na)); %D21=BRLr(na+(1:na),1:na);