Incorrect argument check in h_inf
[scilab.git] / scilab / modules / cacsd / macros / h_inf.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA - F. Delebecque
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 function [Sk,rk,mu]=h_inf(P,r,mumin,mumax,nmax)
11 // H-infinity optimal control for the continuous-time plant P
12 // P is the plant (linear system)given in state-space form or in transfer form,
13 // e.g. P=syslin('c',A,B,C,D) with A,B,C,D = scalar matrices
14 // or P=syslin('c',H) with H a transfer matrix.
15 // r = size of the P22 plant i.e. 2-vector [#outputs,#inputs];
16 // mumin,mumax = bounds on mu with mu=1/gama^2; (mumin=0  usually)
17 // nmax = maximum number of iterations in the gama-iteration.
18 // Two possible calling sequences:
19 // [Sk,mu]=h_inf(P,r,mumin,mumax,nmax) returns mu and the central controller
20 // Sk in the same representation as P. (All calculations being done in state
21 // space).
22 // [Sk,rk,mu]=h_inf(P,r,mumin,mumax,nmax) returns mu
23 //            and the parametrization of all stabilizing controllers:
24 //  a stabilizing controller K is obtained by K=Fl(Sk,r,PHI) where
25 //  PHI is a linear system with dimensions r' and satisfy h_norm(PHI) < gama.
26 //  rk (=r) is the size of the Sk22 block and mu = 1/gama^2 after nmax
27 //  iterations.
28 //!
29 //
30 // Initialization : Uci and Yci normalize P
31 // mu_inf upper bound on mu = gama^-2
32 //P2 = normalized P.
33 //
34   if argn(2)<>5 then
35     error(msprintf(gettext("%s: Wrong number of input arguments: %d expected.\n"),"h_inf",5))
36   end
37
38   if and(typeof(P)<>['rational','state-space']) then
39     error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space or a transfer function expected.\n"),"h_inf",1))
40   end
41   if P.dt<>"c" then
42     error(msprintf(gettext("%s: Wrong value for input argument #%d: Continuous time system expected.\n"),"h_inf",1))
43   end
44   if typeof(r)<>"constant"|~isreal(r) then
45     error(msprintf(gettext("%s: Wrong type for argument %d: Real vector expected.\n"),"h_inf",2))
46   end
47   if size(r,'*')<>2 then
48     error(msprintf(gettext("%s: Wrong size for input argument #%d: %d expected.\n"),"h_inf",2,2))
49   end
50   r=int(r);
51   if or(r<=0) then
52     error(msprintf(gettext("%s: Wrong values for input argument #%d: Elements must be positive.\n"),"h_inf",2))
53   end
54   
55   
56   if typeof(mumin)<>"constant"|~isreal(mumin) then
57     error(msprintf(gettext("%s: Wrong type for argument %d: Real vector expected.\n"),"h_inf",3))
58   end
59   if size(mumin,'*')<>1 then
60     error(msprintf(gettext("%s: Wrong size for input argument #%d: Scalar expected.\n"),"h_inf",3))
61   end
62   if or(mumin<0) then
63     error(msprintf(gettext("%s: Wrong values for input argument #%d: Elements must be positive.\n"),"h_inf",3))
64   end
65
66   
67   if typeof(mumax)<>"constant"|~isreal(mumax) then
68     error(msprintf(gettext("%s: Wrong type for argument %d: Real vector expected.\n"),"h_inf",4))
69   end
70   if size(mumax,'*')<>1 then
71     error(msprintf(gettext("%s: Wrong size for input argument #%d: Scalar expected.\n"),"h_inf",4))
72   end
73   if mumax<0 then
74     error(msprintf(gettext("%s: Wrong values for input argument #%d: Elements must be positive.\n"),"h_inf",4))
75   end
76   if typeof(nmax)<>"constant"|~isreal(nmax) then
77     error(msprintf(gettext("%s: Wrong type for argument %d: Real vector expected.\n"),"h_inf",5))
78   end
79
80   if nmax<>int(nmax)|nmax<=0 then
81     error(msprintf(gettext("%s: Wrong size for input argument #%d: A positive integer expected.\n"),"h_inf",5))
82   end
83   [P2,mu_inf,Uci,Yci,D22]=h_init(P,r,%t)
84   //if mu_inf < mumax then write(%io(2),mu_inf,'(3x,''romax too big: max romax= '',f10.5)');end
85   mumax=min(mu_inf,mumax)
86   //
87   //    Gama-iteration P6 = transformed P2 with D11 removed
88   [P6,Finf,mu,Uc#i,Yc#i]=h_iter(P2,r,mumin,mumax,nmax)
89   if mu==0 then
90     warning(msprintf(_("%s : No feasible ro in bounds [%g  %g]\n"),"h_inf",mumin,mumax));
91     rk=[];Sk=[];
92     return,
93   end
94   //
95   //    Optimal controller for P6
96   [Sk,polesH,polesJ]=h_contr(P6,r,1/mu,Uc#i,Yc#i);
97   [E,Ak,Bk1,Bk2,Ck1,Ck2,Dk11,Dk12,Dk21,Dk22]=Sk(:);
98   //    Add the optimal controller at infinity
99   if norm(Finf,1) <> 0 then Dk11=Dk11+Finf;end
100   //    Back-normalization of the controller
101   Bk1=Bk1*Yci;
102   Ck1=Uci*Ck1;
103   Dk11=Uci*Dk11*Yci;
104   Dk12=Uci*Dk12;
105   Dk21=Dk21*Yci;
106   //Convert to descriptor form:
107   Sk=des2ss(Ak,[Bk1,Bk2],[Ck1;Ck2],[Dk11,Dk12;Dk21,Dk22],E);
108   if argn(1)<3 then
109     Sk=Sk(1:r(2),1:r(1));rk=mu;
110     //    Case D22 different from zero
111     if norm(coeff(D22),1) <> 0 then Sk=Sk/.D22;end
112   else
113     rk=r;
114   end
115   //    Sk in transfer representation if P is.
116   if typeof(P)=='rational' then Sk=ss2tf(Sk);end;
117 endfunction
118
119 function [P2,mu_inf,Uci,Yci,D22]=h_init(P,r,info)
120 //******************************
121 // Initialization of the standard plant
122 // P = standard plant; r=size of P22 (1X2 vector)
123 // P2 = normalized plant
124 // gama_inf :lower bound on gama
125 //
126 //   [C1 D12]'*[C1 D12] = Q = [Q1 S;S' Q2]
127 //   [B1;D21] *[B1;D21]'= R = [R1 L';L R2]
128 //!
129
130   P1=P(1);
131   if P1(1)=='r' then P=tf2ss(P);end
132   [A,B1,B2,C1,C2,D11,D12,D21,D22]=smga(P,r);
133   [na,na]=size(A);
134   [p1,m2]=size(D12),
135   [p2,m1]=size(D21),
136   //Useful indexes
137   l1=1:p1-m2;k1=1:m1-p2;
138   l2=1+p1-m2:p1;k2=1+m1-p2:m1;
139
140   //**************Check assumptions******************
141
142   //Stabilizability/detectability of P22 ?
143
144   P22=syslin('c',A,B2,C2);
145
146   [ns,Us,St]=st_ility(P22,1.d-10)
147
148   if ns<>na then
149     warning(msprintf(gettext("%s:  %s is not stabilizable.\n"),"h_inf","P22"));
150   end
151   if ns==na & info then
152     mprintf(gettext("%s: %s is stabilizable.\n"),"h_inf","P22");
153   end
154
155   [nd,Ud,Sd]=dt_ility(P22,1.d-10)
156
157   if nd <> 0 then
158     warning(msprintf(gettext("%s:   %s is not detectable.\n"),"h_inf","P22"));
159   end
160   if nd==0 & info then
161     mprintf(gettext("%s: %s is detectable.\n"),"h_inf","P22");
162   end
163
164   // rank P21=[A,B2,C1,D12] = m2 ?
165   P12=syslin('c',A,B2,C1,D12);
166   [nt,dt]=trzeros(P12),rzt=real(nt./dt),
167   if size(nt,'*') > 0 then
168     if min(abs(rzt)) < sqrt(%eps) then
169       warning(msprintf(gettext("%s: %s has a zero on/close the imaginary axis.\n"),"h_inf","P12")),
170     end,
171   end,
172
173   // rank P21=[A,B1,C2,D21] = p2 ?
174   P21=syslin('c',A,B1,C2,D21);
175   [nt,dt]=trzeros(P21),rzt=real(nt./dt),
176   if size(nt,'*')>0 then
177     if min(abs(rzt)) < sqrt(%eps) then
178       warning(msprintf(gettext("%s: %s has a zero on/close the imaginary axis.\n"),"h_inf","P21")),
179     end,
180   end,
181
182   //***********************end***************************
183
184   //Row compression of D12 (bottom)
185   [T1,r1]=rowcomp(D12),
186   if r1<>m2 then
187     error(msprintf(gettext("%s: Wrong values for input argument #%d:  %s is not full rank.\n"),"h_inf",1,"D12"));
188   end,
189   T1=[T1(r1+1:p1,:);T1(1:r1,:)],
190   D12=T1*D12,
191   //Column compression of D21 (right)
192   [S1,r2]=colcomp(D21),
193   if r2<>p2 then
194     error(msprintf(gettext("%s: Wrong values for input argument #%d:  %s is not full rank.\n"),"h_inf",1,"D21"));
195   end,
196   D21=D21*S1,
197   //Updating
198   B1=B1*S1,C1=T1*C1,
199   D11=T1*D11*S1,
200
201   // Scaling on U and Y
202   Uc=D12(l2,:);
203   Uci=inv(Uc);
204   B2=B2*Uci;
205   D12=D12*Uci;
206
207   Yc=D21(:,k2)
208   Yci=inv(Yc);
209   C2=Yci*C2;
210   D21=Yci*D21;
211
212   //P2=[A,B1,B2,C1,C2,D11,D12,D21,D22] with D12 and D21 scaled;
213
214   //Initialization
215
216   //Solve H-infinity problem at infinity
217
218   D1111=D11(l1,k1),
219   D1112=D11(l1,k2),
220   D1121=D11(l2,k1),
221   D1122=D11(l2,k2),
222
223   M11=[D1111,D1112];M22=[D1111',D1121'];
224   g1=-1;g2=-1;
225   if M11<>[] then g1=norm(M11);end
226   if M22<>[] then g2=norm(M22);end
227
228   gama_inf=max(g1,g2);
229   if gama_inf==0 then mu_inf=1/%eps/%eps, else mu_inf=1/(gama_inf*gama_inf);end
230
231   P2=syslin('c',A,[B1,B2],[C1;C2],[D11,D12;D21,0*D22]);
232
233   //P2 = standard plant with D22=0 and D12,D21 normalized;
234
235
236 endfunction
237 function [P6ad,Finfad,muad,Uc#iad,Yc#iad]=h_iter(P2,r,mumin,mumax,nmax)
238   niter=0;muad=0;P6ad=[]; Finfad=[];Uc#iad=[];Yc#iad=[];
239   while niter < nmax
240     niter=niter+1;
241     mu=(mumin+mumax)/2;
242     [P6,Finf,tv,Uc#i,Yc#i]=h_test(P2,r,mu)
243
244     test=max(tv)
245
246     if test > 0 then
247       mumax=mu
248     else
249       mumin=mu,muad=mu;P6ad=P6;Finfad=Finf;Uc#iad=Uc#i;Yc#iad=Yc#i;
250     end
251
252   end  //while
253 endfunction
254
255 function [P6,Kinf,tv,Uc#i,Yc#i]=h_test(P2,r,mu)
256 //****************************
257 //To test if mu is feasable for the plant P2 :
258 //mu is feasible for P2 iff the three components of
259 //tv are negative
260 //!
261 //
262 //   [C1 D12]*[C1 D12]'=Q=[Q1 S;S' Q2]
263 //   [B1;D21]*[B1;D21]'=R=[R1 L';L R2]
264   tv=[1,1,1];
265   [A,B1,B2,C1,C2,D11,D12,D21,D22]=smga(P2,r);
266   [p1,m2]=size(D12),
267   [p2,m1]=size(D21),
268   //Useful indexes
269   l1=1:p1-m2;k1=1:m1-p2;
270   l2=1+p1-m2:p1;k2=1+m1-p2:m1;
271   //
272
273   D1111=D11(l1,k1),
274   D1112=D11(l1,k2),
275   D1121=D11(l2,k1),
276   D1122=D11(l2,k2),
277
278   if mu==0 then mu=%eps*%eps;end
279   mu1=1/mu;
280   gama=1/sqrt(mu);
281   gam2=1/(gama*gama);
282
283   err=(m1-p2)*(p1-m2);
284
285   if err==0 then
286     Kinf=-D1122
287   else
288     Kinf=-(D1122+D1121*inv(mu1*eye()-D1111'*D1111)*D1111'*D1112);
289   end
290
291   //Kinf=admissible controller for mu
292
293   A=A+B2*Kinf*C2;
294   B1=B1+B2*Kinf*D21;
295   C1=C1+D12*Kinf*C2;
296   D11=D11+D12*Kinf*D21;
297
298   if norm(D11) >= gama then
299     P6=[]; Kinf=[];Uc#i=[];Yc#i=[];
300     error(msprintf(gettext("%s: gamma too small.\n"),"h_inf"));
301   end
302
303   //P3=list(A,B1,B2,C1,C2,D11,D12,D21,D22) with norm(D11) < gama.
304
305   Teta11=gam2*D11;
306   Teta22=gam2*D11';
307   W12=eye()-gam2*D11*D11';
308   W21=eye()-gam2*D11'*D11;
309   Teta12=(1/gama)*sqrtm(0.5*(W12+W12'));
310   Teta21=-(1/gama)*sqrtm(0.5*(W21+W21'));
311
312   //Teta*Teta'=(1/gama*gama)*eye()
313
314   M=inv(eye()-D11*Teta22);
315   N=inv(eye()-Teta22*D11);
316
317   A=A+B1*Teta22*M*C1;
318
319   B2=B2+B1*Teta22*M*D12;
320   B1=B1*N*Teta21;
321
322   C2=C2+D21*Teta22*M*C1;
323   C1=Teta12*M*C1;
324
325   D11=0*D11;     //By construction...
326
327   D22#=D22+D21*Teta22*M*D12;
328
329   D12=Teta12*M*D12;
330   D21=D21*N*Teta21;
331
332   //P4 =syslin('c',A,[B1,B2],[C1;C2],[D11,D12;D21,D22]
333   //          with D11=0; P4=Fl(Teta,size(D11'),P3,r);
334
335   D22=0*D22#;
336
337   //P5 = [A,[B1,B2],[C1;C2],[D11,D12;D21,D22] with D11=0 and D22=0;
338
339   //Row compression of D12
340   [T1,r1]=rowcomp(D12);
341   if r1<>m2 then
342     error(msprintf(gettext("%s: Wrong values for input argument #%d:  %s is not full rank.\n"),"h_inf",1,"D12"));
343   end
344   T1=[T1(r1+1:p1,:);T1(1:r1,:)],
345   D12=T1*D12,
346   //Column compression of D21
347   [S1,r2]=colcomp(D21),
348   if r2<>p2 then
349     error(msprintf(gettext("%s: Wrong values for input argument #%d:  %s is not full rank.\n"),"h_inf",1,"D21"));
350   end,
351   D21=D21*S1,
352   //Updating
353   B1=B1*S1,C1=T1*C1,
354   D11=T1*D11*S1,
355   // Scaling on U and Y
356   Uc#=D12(l2,:);
357   Uc#i=inv(Uc#);
358   B2=B2*Uc#i;
359   D12=D12*Uc#i;
360
361   Yc#=D21(:,k2)
362   Yc#i=inv(Yc#);
363   C2=Yc#i*C2;
364   D21=Yc#i*D21;
365
366   //P6=[A,B1,B2,C1,C2,D11,D12,D21,D22] with D11=0,D22=0;D12 and D21 scaled.
367   //Standard assumptions now satisfied
368
369   //P6=[A,B1,B2,C1,C2,D11,D12,D21,D22];
370
371   //     Test of mu for P6  <=> Test of mu^-1 for P2
372   //Optimal controller :
373   indic=0;
374
375   mu_test=1/mu;
376
377   R1=B1*B1';
378   S=D12'*C1;
379   C1#=(eye()-D12*D12')*C1;
380   Ax=A-B2*S;
381   Qx=-C1#'*C1#;
382
383   Rx=mu_test*R1-B2*B2';
384   H=[Ax Rx;
385      Qx -Ax'];
386
387   dx=min(abs(real(spec(H))));
388   //write(%io(2),dx);
389   if dx < 1.d-9 then
390     mprintf(gettext("%s: An eigenvalue of %s (controller) is close to Imaginary axis.\n"),"h_inf","H");
391     write(%io(2),dx);
392     indic=1;test=1;
393   end
394   if indic ==0 then
395     [X1,X2,errx]=ric_desc(H);
396     if errx > 1.d-4 then
397       mprintf(gettext("%s: Riccati solution inaccurate: equation error = %g.\n"),"h_inf",errx);
398     end
399     //Optimal observer :
400
401     Q1=C1'*C1;
402     L=B1*D21';
403     B1#=B1*(eye()-D21'*D21);
404     Ay=A-L*C2;
405     Qy=-B1#*B1#';
406
407     Ry=mu_test*Q1-C2'*C2;
408
409     J=[Ay' Ry;
410        Qy -Ay];
411     dy=min(abs(real(spec(J))));
412     //write(%io(2),dy);
413     if dy < 1.d-9 then
414       mprintf(gettext("%s: An eigenvalue of %s (observer) is close to Imaginary axis.\n"+..
415                       "The distance to the imaginary axis is less than %g "),"h_inf","J",1.d-9);
416       write(%io(2),dy);
417       indic=1 ;test=1;
418     end
419     if indic==0 then
420       [Y1,Y2,erry]=ric_desc(J);
421       if erry > 1.d-4 then
422         mprintf(gettext("%s: Riccati solution inaccurate: equation error = %g.\n"),"h_inf",erry);
423       end
424       //Tests
425       //
426       //     E=(Y2'*X2-mu*Y1'*X1);
427       //     write(%io(2),min(svd(E)),'(5x,''min(svd(E)) = '',f10.2)')
428       [al1,be1]=spec(A*X2 -B2*(S*X2 +B2'*X1 ),X2);
429       [al2,be2]=spec(Y2'*A-(Y2'*L+Y1'*C2')*C2,Y2');
430       [al3,be3]=spec(mu_test*Y1'*X1,Y2'*X2);
431
432       //Here division by zero may appear...
433       //If such division appear try to uncomment the 3 following lines:
434       w1=find(be1==0);be1(w1)=%eps*ones(be1(w1));
435       w2=find(be2==0);be2(w2)=%eps*ones(be2(w2));
436       w3=find(be3==0);be3(w3)=%eps*ones(be3(w3));
437
438       test1=max(real(al1./be1));
439       test2=max(real(al2./be2));
440       test3=max(real(al3./be3))-1;
441       tv   =[test1,test2,test3]
442     end
443   end
444
445   //write(%io(2),1/sqrt(mu),'(10x,'' Try gama = '',f18.10)');
446   [answer,no]=max(tv);
447   //if exists('tv')==1 then write(%io(2),[tv,max(tv)],'(4f15.10)');end
448   comm1=_("Unfeasible (Hx hamiltonian)")
449   comm2=_("Unfeasible (Hy hamiltonian)")
450   comm3=_("Unfeasible (Hy hamiltonian)")
451   if exists('tv')==1 then
452     if answer>0 then
453       // @TODO This stuff should be localized... To bored to understand it for now.
454       select no
455       case 1  then
456         fmt='('' gama = '',f18.10,'' '+_("Unfeasible (Hx hamiltonian)")+'   test = '',e15.5)'
457       case 2 then
458         fmt='('' gama = '',f18.10,'' '+_("Unfeasible (Hy hamiltonian)")+'   test = '',e15.5)'
459       case 3 then
460         fmt='('' gama = '',f18.10,'' '+_("Unfeasible (spectral radius)")+'  test = '',e15.5)'
461       else
462         fmt='('' gama = '',f18.10,''             ok             )  test = '',e15.5)'
463       end;
464       write(%io(2),[1/sqrt(mu),answer],fmt);
465     end
466   end
467   P6=syslin('c',A,[B1,B2],[C1;C2],[D11,D12;D21,D22])
468 endfunction
469
470 function [Sk,polesH,polesJ]=h_contr(P,r,mu,U2i,Y2i)
471 // ****************************
472 // Computation of the optimal controller Sk for a standard
473 // plant which satisfies the assumption D11=0
474 //
475 // F.D.
476 //!
477 //   [C1 D12]*[C1 D12]'=Q=[Q1 S;S' Q2]
478 //   [B1;D21]*[B1;D21]'=R=[R1 L';L R2]
479 //
480
481   [A,B1,B2,C1,C2,D11,D12,D21,D22]=smga(P,r);
482   if norm(D11,1) > %eps then
483     error('D11 <> 0'),
484   end
485
486   [p2,m1]=size(D21),
487   [p1,m2]=size(D12),
488   l1=1:p1-m2;k1=1:m1-p2;
489   l2=1+p1-m2:p1;k2=1+m1-p2:m1;
490
491   //Initialization  : constants
492
493   R1=B1*B1';
494   S=D12'*C1;
495   C1#=(eye()-D12*D12')*C1;
496   Ax=A-B2*S;
497   Qx=-C1#'*C1#;
498
499   Q1=C1'*C1;
500   L=B1*D21';
501   B1#=B1*(eye()-D21'*D21);
502   Ay=A-L*C2;
503   Qy=-B1#*B1#';
504
505   //mu-dependent part
506
507   //Optimal controller
508
509   Rx=mu*R1-B2*B2';
510
511   H=[Ax Rx;
512      Qx -Ax'];
513   polesH=spec(H);
514   dx=min(abs(real(polesH)));
515   //write(%io(2),dx);
516   if dx < 1.d-6 then
517     mprintf(gettext("%s: An eigenvalue of %s (controller) is close to Imaginary axis.\n"),"h_inf","H");
518
519   end
520   [X1,X2,errx]=ric_desc(H);
521   if errx > 1.d-4 then
522     mprintf(gettext("%s: Riccati solution inaccurate: equation error = %g.\n"),"h_inf",errx);
523   end
524
525   //Optimal observer :
526
527   Ry=mu*Q1-C2'*C2;
528
529   J=[Ay' Ry;
530      Qy -Ay];
531   polesJ=spec(J);
532   dy=min(abs(real(polesJ)));
533   //write(%io(2),dy);
534   if dy < 1.d-6 then
535     mprintf(gettext("%s: An eigenvalue of %s (observer) is close to Imaginary axis.\n"),"h_inf","J");
536   end
537   [Y1,Y2,erry]=ric_desc(J);
538   if erry > 1.d-4 then
539     mprintf(gettext("%s: Riccati solution inaccurate: equation error = %g.\n"),"h_inf",erry);
540   end
541
542   //Controller in descriptor form
543
544   E=(Y2'*X2-mu*Y1'*X1);
545   A#=A-B2*S-L*C2;
546   Ak=Y2'*A#*X2+mu*Y1'*A#'*X1-Y2'*(mu*Qy+B2*B2')*X1-Y1'*(mu*Qx+C2'*C2)*X2;
547   Bk1=(Y2'*L+Y1'*C2');
548   Ck1=-(S*X2+B2'*X1);
549
550   Bk2=Y2'*B2+Y1'*S'
551   Ck2=-(C2*X2+L'*X1)
552
553   Dk11=0*Ck1*Bk1;
554   Dk22=0*Ck2*Bk2;
555   Dk12=eye(Ck1*Bk2);
556   Dk21=eye(Ck2*Bk1);
557
558   //Scaling back
559
560   Bk1=Bk1*Y2i;
561   Ck1=U2i*Ck1;
562   Dk21=Dk21*Y2i;
563   Dk12=U2i*Dk12;
564   //  Dk11=U2i*Dk11*Y2i
565
566   Sk=list(E,Ak,Bk1,Bk2,Ck1,Ck2,Dk11,Dk12,Dk21,Dk22);
567 endfunction