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