diff options
Diffstat (limited to 'modules/cacsd/macros/ccontrg.sci')
| -rwxr-xr-x | modules/cacsd/macros/ccontrg.sci | 228 |
1 files changed, 228 insertions, 0 deletions
diff --git a/modules/cacsd/macros/ccontrg.sci b/modules/cacsd/macros/ccontrg.sci new file mode 100755 index 000000000..41f6d1fb9 --- /dev/null +++ b/modules/cacsd/macros/ccontrg.sci @@ -0,0 +1,228 @@ +// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab +// Copyright (C) INRIA - +// +// This file must be used under the terms of the CeCILL. +// This source file is licensed as described in the file COPYING, which +// you should have received as part of this distribution. The terms +// are also available at +// http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt + + +function [K]=ccontrg(PP,r,Gamma); + //*********************************** + // returns a realization of the central controller for the + // general problem using the formulas in Gahinet, 92 + // Note that Gamma must be > gopt (output of gamitg) + + + // PP contains the parameters of plant realization (sylin list) + // b = ( b1 , b2 ) , c = ( c1 ) , d = ( d11 d12) + // ( c2 ) ( d21 d22) + // r(1) and r(2) are the dimensions of d22 (rows x columns) + if argn(2)<>3 then + error(msprintf(gettext("%s: Wrong number of input arguments: %d expected.\n"),"ccontrg",3)) + end + + if typeof(PP)<>"state-space" then + error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"ccontrg",1)) + end + if PP.dt<>"c" then + error(msprintf(gettext("%s: Wrong value for input argument #%d: Continuous time system expected.\n"),"ccontrg",1)) + end + if typeof(r)<>"constant"|~isreal(r) then + error(msprintf(gettext("%s: Wrong type for argument #%d: Real vector expected.\n"),"ccontrg",2)) + end + if size(r,"*")<>2 then + error(msprintf(gettext("%s: Wrong size for input argument #%d: %d expected.\n"),"ccontrg",2,2)) + end + r=int(r); + if or(r<=0) then + error(msprintf(gettext("%s: Wrong values for input argument #%d: Elements must be positive.\n"),"ccontrg",2)) + end + + //parameter recovery + [a,b1,b2,c1,c2,d11,d12,d21,d22]=smga(PP,r); + + //dimensions + [na,na]=size(a); nh=2*na; + [p1,m2]=size(d12), + [p2,m1]=size(d21), + gs=Gamma**2; + + //HAMILTONIAN SETUP + //------------------ + sh22=m1+m2; + h11=[a,0*eye(a);-c1'*c1,-a']; + h12=[-b1,-b2;c1'*d11,c1'*d12]; + h21=[d11'*c1,b1';d12'*c1,b2']; + h22=gs*eye(m1,m1); h22(sh22,sh22)=0; + h22=h22-[d11,d12]'*[d11,d12]; + + sj22=p1+p2; + j11=[a',0*eye(a);-b1*b1',-a]; + j12=[-c1',-c2';b1*d11',b1*d21'] + j21=[d11*b1',c1;d21*b1',c2]; + j22=gs*eye(p1,p1); j22(sj22,sj22)=0; + j22=j22-[d11;d21]*[d11;d21]'; + + + + //computation of Xinf and Yinf + //----------------------------- + + //compute orthon. bases of the negative inv. subspaces. + + [q,r]=qr([h12;h22]); + q12=q(1:nh,sh22+1:nh+sh22); q22=q(nh+1:nh+sh22,sh22+1:nh+sh22); + hr=q12'*h11+q22'*h21; + [uh,dh]=schur(hr,q12',"c"); + px=uh(1:na,1:na); qx=uh(na+1:nh,1:na); + + [q,r]=qr([j12;j22]); + q12=q(1:nh,sj22+1:nh+sj22); q22=q(nh+1:nh+sj22,sj22+1:nh+sj22); + jr=q12'*j11+q22'*j21; + [uj,dj]=schur(jr,q12',"c"); + py=uj(1:na,1:na); qy=uj(na+1:nh,1:na); + + + //computation of M,N + [uz,sz,vz]=svd(qx'*qy/gs-px'*py); + sz=sqrt(sz); + + + //DETERMINATION OF DK + //------------------- + + [u,s,v]=svd(d12); r12=max(size(find(diag(s) > 1.0e-10))); + [w,p,z]=svd(d21); r21=max(size(find(diag(p) > 1.0e-10))); + u1=u(:,1:r12); v1=v(:,1:r12); w1=w(:,1:r21); z1=z(:,1:r21); + s1=s(1:r12,1:r12); ph1=p(1:r21,1:r21); + d11tr=u'*d11*z; + + [g0,d0]=parrt(d11tr(r12+1:p1,r21+1:m1),d11tr(r12+1:p1,1:r21),.. + d11tr(1:r12,r21+1:m1),r12,r21); + dk=v1*s1\(d0-d11tr(1:r12,1:r21))/ph1*w1'; + + + //DETERMINATION OF BK, CK + //----------------------- + + hd11=(eye(p1,p1)-u1*u1')*d11; + hb1=b1-b2*v1*(s1\(u1'*d11)); + that=dk*c2*px+v1*s1\(u1'*c1*px+s1\v1'*b2'*qx+.. + (d0*z1'+u1'*d11*(eye(m1,m1)-z1*z1'))*.. + ((gs*eye(m1,m1)-hd11'*hd11)\(hb1'*qx+hd11'*c1*px))); + + td11=d11*(eye(m1,m1)-z1*z1'); + tc1=c1-(d11*z1/ph1)*w1'*c2; + ttil=py'*b2*dk+(py'*b1*z1+qy'*c2'*w1/ph1+.. + ((qy'*tc1'+py'*b1*td11')/(gs*eye(p1,p1)-td11*td11'))*.. + ((eye(p1,p1)-u1*u1')*d11*z1+u1*d0))/ph1*w1'; + + ck=-that*uz/sz; bk=-sz\vz'*ttil; + + + //just checking... + x=qx/px; y=qy/py; + d12p=pinv(d12); d21p=pinv(d21); + thh=d12p*(c1+d12p'*b2'*x)+dk*c2+(d12p*d11+dk*d21)/.. + (gs*eye(m1,m1)-hd11'*hd11)*((b1-b2*d12p*d11)'*x+hd11'*c1); + thh=thh*px; + ttt=(b1+y*c2'*d21p')*d21p+b2*dk+(y*tc1'+b1*td11')/.. + (gs*eye(p1,p1)-td11*td11')*(d11*d21p+d12*dk); + ttt=py'*ttt; + + nmin=max(norm(hd11),norm(td11)); + ncom=norm(d11+d12*dk*d21); + + + //DETERMINATION OF AK + //------------------- + + ca=a+b2*dk*c2; cb=b1+b2*dk*d21; cc=c1+d12*dk*c2; Cd=d11+d12*dk*d21; + ak=py'*b2*that+ttil*c2*px-py'*ca*px-qy'*ca'*qx/gs+.. + [-qy'*cc'/Gamma,py'*cb-ttil*d21]/.. + [Gamma*eye(p1,p1),Cd;Cd',Gamma*eye(m1,m1)]*.. + [cc*px-d12*that;-cb'*qx/Gamma]; + ak=sz\(vz'*ak*uz)/sz; + + + + K=syslin("c",ak,bk,ck,dk); + +endfunction + +function [go,xo]=parrt(a,b,c,rx,cx); + // + // [go,xo]=par(a,b,c,rx,cx) solves the minimization (Parrot) problem: + // + // || a b || + // min || || + // X || c x ||2 + // + // an explicit solution is + // 2 T -1 T + // Xo = - c ( go I - a a) a b + // where T T + // go = max ( || (a , b) || , || (a , c) || ) + // + // rx and cx are the dimensions of Xo (optional if a is nondegenerate) + // + //! + //constant + TOLA=1.0e-8; // threshold used to discard near singularities in gs I - A'*A + + go=max(norm([a b]),norm([a;c])); + [ra,cb]=size(b); [rc,ca]=size(c); xo=0; + + + //MONITOR LIMIT CASES + //-------------------- + if ra==0 | ca == 0 | go == 0 then xo(rx,cx)=0; return; end + + + + //COMPUTE Xo IN NONTRIVIAL CASES + //------------------------------ + + gs=(go**2); + [u,s,v]=svd(a); + + //form gs I - s' * s + ns=min(ra,ca); + if size(s,1)>1 then + d=diag(s); + else + d=s(1) + end + dd=gs*ones(d)-d**2; + + + //isolate the non (nearly) singular part of gs I - A'*A + nnz1=nthresh(dd/go,TOLA); + nd=ns-nnz1; //number of singular values thresholded out + + //compute xo + if nnz1==0 then + xo(rc,cb)=0; + else + unz=u(:,nd+1:ra); + vnz=v(:,nd+1:ca); + s=s(nd+1:ra,nd+1:ca); + for i=1:nnz1 + s(i,i)=s(i,i)/dd(i+nd); + end + xo=-c*vnz*s'*unz'*b; + end +endfunction + +function n=nthresh(d,tol) + n=find(d<=tol,1) + if n==[] then + n=size(d,"*"), + else + n=n-1 + end +endfunction + + |