90513b01d10f34f7dd5c6edb527f65f2bdb08dfe
[scilab.git] / scilab / modules / cacsd / demos / lqg / lqg.dem
1 //
2 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 // Copyright (C) ????-2008 - INRIA
4 //
5 // This file is distributed under the same license as the Scilab package.
6 //
7
8 mode(-1);
9 oldln=lines();lines(0)
10 //display the diagram
11 x=[5,10,20,40,50,70,80,90];xmin=-10;xmax=100;
12 y=[22,28,30,32];ymin=12;ymax=40;
13
14 xx=[x([1 2 2 7 4 6 3 4 5 6 3 3 5 5]);x([3,2,7,7,5,8,3,4,5,6,4,4,6,6])];
15 yy=[y([3,1,1,1,3,3,2,2,2,2,2,4,2,4]);y([3,3,1,3,3,3,4,4,4,4,2,4,2,4])];
16
17 scf(0);clf();show_window();
18 a=gca();a.data_bounds=[xmin ymin;xmax ymax];a.box='on';
19 xpolys(xx,yy)
20 xstring(28,30,'K');xstring(56,30,'Plant');xstring(12,28.80,'-');
21 xtitle('PLANT   and   CONTROLLER')
22
23
24 // xset("window",0);clf();show_window();
25 // plot2d(xx,yy,ones(1,16),'022');
26 // xstring(28,30,'K');xstring(56,30,'Plant');xstring(12,28.80,'-');
27 // xtitle('PLANT   and   CONTROLLER')
28
29 path=get_absolute_file_path('lqg.dem');
30 s=poly(0,'s');z=poly(0,'z');
31 messagebox(['Simple example of SISO LQG Design';
32            'Demo is in file '+path+'lqg.dem';
33            'Computes the LQG compensator and plots response'],"modal");
34
35 n=x_choose(['Continuous time';'Discrete time'],'Select time domain');
36
37 select n
38 case 0
39   return
40 case 1
41   mode(1)
42   s=poly(0,'s');
43   str='(s-1)/(s^2-5*s+1)';
44   rep=x_dialog('Nominal plant?',str)
45   if rep==[] then return,end
46   Plant=evstr(rep);
47   Plant=syslin('c',Plant);
48   mode(-1)
49 case 2
50   mode(1)
51   z=poly(0,'z');
52   str='(z+1)/(z^2-5*z+2)'
53   rep=x_dialog('Nominal plant?',str)
54   if rep==[] then return,end
55   Plant=evstr(rep);
56   Plant=syslin('d',Plant);
57   mode(-1)
58 end
59
60 mode(1)
61
62 //Nominal Plant
63
64 P22=tf2ss(Plant);    //...in state-space form
65 [ny,nu,nx]=size(P22);
66 messagebox('Now enter weighting matrices',"modal");
67 rep=x_matrix('x-weighting matrix',eye(nx,nx))
68 if rep==[] then return,end
69 Qx=evstr(rep);
70 rep=x_matrix('u-weighting matrix',eye(nu,nu));
71 if rep==[] then return,end
72 Qu=evstr(rep);
73 bigQ=sysdiag(Qx,Qu);
74 rep=x_matrix('x-noise covariance matrix',eye(nx,nx))
75 if rep==[] then return,end
76 Rx=evstr(rep);
77 rep=x_matrix('y-noise covariance matrix',eye(ny,ny))
78 if rep==[] then return,end
79 Ry=evstr(rep);
80 bigR=sysdiag(Rx,Ry);
81 [Plqg,r]=lqg2stan(P22,bigQ,bigR);     //LQG pb as a standard problem
82 Klqg=lqg(Plqg,r);                     //LQG compensator
83
84 disp(spec(h_cl(Plqg,r,Klqg)),'closed loop eigenvalues:');    //Check internal stability
85 [Slqg,Rlqg,Tlqg]=sensi(P22,Klqg);  //Sensitivity functions
86
87 disp(clean(ss2tf(Slqg)),'Sensitivity function');
88 disp(clean(ss2tf(Tlqg)),'Complementary sensitivity function');
89
90 mode(-1);
91
92 messagebox('Closed-loop response',"modal");
93
94 resp=['Frequency response';'Time response'];
95 while %t do
96   n=x_choose(resp,'Select response(s)');
97   select n
98   case 0
99     disp("LQG demo stops!");break;
100   case 1 then
101     mode(1)
102     scf(0);clf();show_window();bode(Tlqg);
103     mode(-1)
104   case 2
105     if Plant(4)=='c' then
106       mode(1)
107       defv=['0.1','20'];
108       title='Enter Sampling period and Tmax';
109       rep=x_mdialog(title,['Sampling period?';'Tmax?'],defv)
110       if rep==[] then break,end
111       dttmax=evstr(rep)
112       dt=evstr(dttmax(1));tmax=evstr(dttmax(2));
113       t=0:dt/5:tmax;
114       n1=x_choose(['Step response?';'Impulse response?'],'Simulation:');
115       select n1
116       case 1 then
117         scf(1);clf();show_window();
118         plot2d([t',t'],[(csim('step',t,Tlqg))',ones(t')]);
119       case 2 then
120         scf(1);clf();show_window();
121         plot2d([t',t'],[(csim('impul',t,Tlqg))',0*t']);
122       end
123       mode(-1)
124     elseif Plant(4)=='d' then
125       mode(1)
126       defv=['30'];
127       title='Tmax?';
128       rep=x_mdialog(title,['Tmax='],defv)
129       if rep==[] then break,end
130       Tmax=evstr(rep);
131       mode(-1)
132       n2=x_choose(['Step response?';'Impulse response?'],'Simulation:');
133       select n2
134       case 0 then
135         break;
136       case 1 then
137         mode(1)
138         u=ones(1,Tmax);u(1)=0;
139         scf(1);clf();show_window();
140         plot2d([(1:Tmax)',(1:Tmax)'],[(dsimul(Tlqg,u))',(ones(1:Tmax)')])
141         mode(-1)
142       case 2 then
143         mode(1)
144         u=zeros(1,Tmax);u(1)=1;
145         scf(1);clf();show_window();
146         plot2d((1:Tmax)',(dsimul(Tlqg,u))')
147         mode(-1)
148       end
149     end
150   end
151 end
152 lines(oldln(2))
153 clear s z n str rep Plant P22 ny nu nx Qx Qu bigQ Rx Ry bigR  Plqg r 
154 clear Klqg Slqg Rlqg Tlqg resp title dttmax dt t n1 defv Tmax n2 u oldln