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