bug 4319 fix (ccontrg)
[scilab.git] / scilab / modules / cacsd / macros / ccontrg.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA - 
3 // 
4 // This file must be used under the terms of the CeCILL.
5 // This source file is licensed as described in the file COPYING, which
6 // you should have received as part of this distribution.  The terms
7 // are also available at    
8 // http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
9
10
11 function [K]=ccontrg(PP,r,Gamma);
12 //***********************************
13 //   returns a realization of the central controller for the
14 //   general problem using the formulas in Gahinet, 92
15 //   Note that Gamma must be > gopt (ouput of gamitg)
16
17
18 //  PP contains the parameters of plant realization (sylin list)
19 //  b = ( b1 , b2 ) ,   c = ( c1 ) ,    d = ( d11  d12)
20 //                          ( c2 )          ( d21  d22)
21 //  r(1) and r(2) are the dimensions of d22 (rows x columns)
22   
23   
24 //parameter recovery
25   [a,b1,b2,c1,c2,d11,d12,d21,d22]=smga(PP,r);
26   
27   //dimensions
28   [na,na]=size(a); nh=2*na;
29   [p1,m2]=size(d12),
30   [p2,m1]=size(d21),
31   gs=Gamma**2;
32   
33   //HAMILTONIAN SETUP
34   //------------------
35   sh22=m1+m2;
36   h11=[a,0*eye(a);-c1'*c1,-a'];
37   h12=[-b1,-b2;c1'*d11,c1'*d12];
38   h21=[d11'*c1,b1';d12'*c1,b2'];
39   h22=gs*eye(m1,m1); h22(sh22,sh22)=0;
40   h22=h22-[d11,d12]'*[d11,d12];
41   
42   sj22=p1+p2;
43   j11=[a',0*eye(a);-b1*b1',-a];
44   j12=[-c1',-c2';b1*d11',b1*d21']
45   j21=[d11*b1',c1;d21*b1',c2];
46   j22=gs*eye(p1,p1); j22(sj22,sj22)=0;
47   j22=j22-[d11;d21]*[d11;d21]';
48   
49   
50   
51   //computation of Xinf and Yinf
52   //-----------------------------
53   
54   //compute orthon. bases of the negative inv. subspaces.
55   
56   [q,r]=qr([h12;h22]);
57   q12=q(1:nh,sh22+1:nh+sh22); q22=q(nh+1:nh+sh22,sh22+1:nh+sh22);
58   hr=q12'*h11+q22'*h21;
59   [uh,dh]=schur(hr,q12','c');
60   px=uh(1:na,1:na);   qx=uh(na+1:nh,1:na);
61   
62   [q,r]=qr([j12;j22]);
63   q12=q(1:nh,sj22+1:nh+sj22); q22=q(nh+1:nh+sj22,sj22+1:nh+sj22);
64   jr=q12'*j11+q22'*j21;
65   [uj,dj]=schur(jr,q12','c');
66   py=uj(1:na,1:na);   qy=uj(na+1:nh,1:na);
67   
68   
69   //computation of M,N
70   [uz,sz,vz]=svd(qx'*qy/gs-px'*py);
71   sz=sqrt(sz);
72   
73   
74   //DETERMINATION OF DK
75   //-------------------
76   
77   [u,s,v]=svd(d12); r12=maxi(size(find(diag(s) > 1.0e-10)));
78   [w,p,z]=svd(d21); r21=maxi(size(find(diag(p) > 1.0e-10)));
79   u1=u(:,1:r12); v1=v(:,1:r12); w1=w(:,1:r21); z1=z(:,1:r21);
80   s1=s(1:r12,1:r12); ph1=p(1:r21,1:r21);
81   d11tr=u'*d11*z;
82
83   [g0,d0]=parrt(d11tr(r12+1:p1,r21+1:m1),d11tr(r12+1:p1,1:r21),..
84                 d11tr(1:r12,r21+1:m1),r12,r21);
85   dk=v1*s1\(d0-d11tr(1:r12,1:r21))/ph1*w1';
86   
87   
88   //DETERMINATION OF BK, CK
89   //-----------------------
90   
91   hd11=(eye(p1,p1)-u1*u1')*d11;
92   hb1=b1-b2*v1*(s1\(u1'*d11));
93   that=dk*c2*px+v1*s1\(u1'*c1*px+s1\v1'*b2'*qx+..
94                        (d0*z1'+u1'*d11*(eye(m1,m1)-z1*z1'))*..
95                        ((gs*eye(m1,m1)-hd11'*hd11)\(hb1'*qx+hd11'*c1*px)));
96   
97   td11=d11*(eye(m1,m1)-z1*z1');
98   tc1=c1-(d11*z1/ph1)*w1'*c2;
99   ttil=py'*b2*dk+(py'*b1*z1+qy'*c2'*w1/ph1+..
100                   ((qy'*tc1'+py'*b1*td11')/(gs*eye(p1,p1)-td11*td11'))*..
101                   ((eye(p1,p1)-u1*u1')*d11*z1+u1*d0))/ph1*w1';
102   
103   ck=-that*uz/sz; bk=-sz\vz'*ttil;
104   
105   
106   //just checking...
107   x=qx/px; y=qy/py;
108   d12p=pinv(d12);  d21p=pinv(d21);
109   thh=d12p*(c1+d12p'*b2'*x)+dk*c2+(d12p*d11+dk*d21)/..
110       (gs*eye(m1,m1)-hd11'*hd11)*((b1-b2*d12p*d11)'*x+hd11'*c1);
111   thh=thh*px;
112   ttt=(b1+y*c2'*d21p')*d21p+b2*dk+(y*tc1'+b1*td11')/..
113       (gs*eye(p1,p1)-td11*td11')*(d11*d21p+d12*dk);
114   ttt=py'*ttt;
115   
116   nmin=maxi(norm(hd11),norm(td11));
117   ncom=norm(d11+d12*dk*d21);
118   
119   
120   //DETERMINATION OF AK
121   //-------------------
122   
123   ca=a+b2*dk*c2; cb=b1+b2*dk*d21; cc=c1+d12*dk*c2; Cd=d11+d12*dk*d21;
124   ak=py'*b2*that+ttil*c2*px-py'*ca*px-qy'*ca'*qx/gs+..
125      [-qy'*cc'/Gamma,py'*cb-ttil*d21]/..
126      [Gamma*eye(p1,p1),Cd;Cd',Gamma*eye(m1,m1)]*..
127      [cc*px-d12*that;-cb'*qx/Gamma];
128   ak=sz\(vz'*ak*uz)/sz;
129   
130   
131   
132   K=syslin('c',ak,bk,ck,dk);
133   
134 endfunction
135
136 function [go,xo]=parrt(a,b,c,rx,cx);
137 //
138 // [go,xo]=par(a,b,c,rx,cx) solves the minimization (Parrot) problem:
139 //
140 //          || a   b ||
141 //     min  ||       ||
142 //      X   || c   x ||2
143 //
144 //  an explicit solution is
145 //                   2      T  -1  T
146 //      Xo = - c ( go  I - a a)   a b
147 //  where                               T   T
148 //      go = max ( || (a , b) || , || (a , c) || )
149 //
150 //  rx and cx are the dimensions of Xo (optional if a is nondegenerate)
151 //
152 //!
153 //constant
154   TOLA=1.0e-8; // threshold used to discard near singularities in gs I - A'*A
155   
156   go=maxi(norm([a b]),norm([a;c]));
157   [ra,cb]=size(b); [rc,ca]=size(c); xo=0;
158   
159   
160   //MONITOR LIMIT CASES
161   //--------------------
162   if ra==0 | ca == 0 | go == 0 then xo(rx,cx)=0; return; end
163   
164   
165   
166   //COMPUTE Xo IN NONTRIVIAL CASES
167   //------------------------------
168   
169   gs=(go**2);
170   [u,s,v]=svd(a);
171   
172   //form  gs I - s' * s
173   ns=mini(ra,ca);
174   if size(s,1)>1 then
175     d=diag(s);
176   else
177     d=s(1)
178   end
179   dd=gs*ones(d)-d**2;
180   
181   
182   //isolate the non (nearly) singular part of gs I - A'*A
183   nnz1=nthresh(dd/go,TOLA);
184   nd=ns-nnz1;   //number of singular values thresholded out
185
186   //compute xo
187   if nnz1==0 then
188     xo(rc,cb)=0;
189     
190   else
191     unz=u(:,nd+1:ra);
192     vnz=v(:,nd+1:ca);
193     s=s(nd+1:ra,nd+1:ca);
194     for i=1:nnz1, s(i,i)=s(i,i)/dd(i+nd); end
195     
196     cross=diag(dd)\(s(nd+1:ra,nd+1:ca))';
197     
198     //cross  now contains the nonsingular part of s / (gs I - s' * s)
199     
200     xo=-c*vnz*s'*unz'*b;
201     
202   end
203 endfunction
204
205 function n=nthresh(d,tol)
206   n=find(d<=tol,1)
207   if n==[] then 
208     n=size(d,'*'),
209   else
210     n=n-1
211   end
212 endfunction
213
214