home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 7.ddi / ROBUST.DI$ / SLTRDEMO.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  9.2 KB  |  243 lines
  1. %
  2. %SLTRDEMO LQG/LTR SISO Design Demonstration
  3. %
  4.  
  5. % R. Y. Chiang & M. G. Safonov 3/88
  6. % Copyright (c) 1988 by the MathWorks, Inc.
  7. % All Rights Reserved.
  8. % ----------------------------------------------------------------
  9. clear
  10. clc
  11. disp(' ')
  12. disp('  ')
  13. disp('  ')
  14. disp('                << LQG/LTR Demo: SISO Fighter Design Example >>')
  15. disp(' ')
  16. disp(' ')
  17. disp('                        ----')
  18. disp('                         \  \          ----')
  19. disp('                       == \ = \         \  \')
  20. disp('                          /     \        \  \')
  21. disp('                    / ---------------------------- \')
  22. disp('                   |         FIGHTER OF 1990        >>>> -----')
  23. disp('                    \ ---------------------------- /')
  24. disp('                          \     /        /  /')
  25. disp('                       == / = /         /  /')
  26. disp('                         /  /          ----')
  27. disp('                        ----')
  28. ag =[
  29. -2.2567e-02  -3.6617e+01  -1.8897e+01  -3.2090e+01   3.2509e+00  -7.6257e-01;
  30. 9.2572e-05  -1.8997e+00   9.8312e-01  -7.2562e-04  -1.7080e-01  -4.9652e-03;
  31. 1.2338e-02   1.1720e+01  -2.6316e+00   8.7582e-04  -3.1604e+01   2.2396e+01;
  32. 0            0   1.0000e+00            0            0            0;
  33. 0            0            0            0  -3.0000e+01            0;
  34. 0            0            0            0            0  -3.0000e+01];
  35. bg = [0     0;
  36.       0     0;
  37.       0     0;
  38.       0     0;
  39.      30     0;
  40.       0    30];
  41. cg = [0     1     0     0     0     0;
  42.       0     0     0     1     0     0];
  43. dg = [0     0;
  44.       0     0];
  45. bg = bg(:,1); cg = cg(2,:); dg = 0;
  46. disp('  ')
  47. disp('  ')
  48. disp('                                 (strike a key to continue ...)')
  49. pause
  50. clc
  51. disp('  ')
  52. disp('   TF of the fighter pitch axis (trimed @ 25000 ft, 0.9 mach):')
  53. disp('   (from elevon actuator to attitude angle output)')
  54. disp('  ')
  55. disp('  ')
  56. disp('                  -(948.12 s^3 + 30325 s^2 + 56482 s + 1215.3)')
  57. disp('G(s) = ------------------------------------------------------------------------')
  58. disp('        s^6 + 64.554 s^5 + 1167 s^4 + 372.86 s^3 - 5495.4 s^2 + 1102 s + 708.1')
  59. [num,den] = ss2tf(ag,bg,cg,dg,1);
  60. disp(' ')
  61. disp(' ')
  62. disp('                                 (strike a key to continue ...)')
  63. pause
  64. clc
  65. disp('  ')
  66. disp('  ')
  67. disp('    Poles of the Plant (unstable Phugoid modes):')
  68. disp('  ')
  69. disp('    ---------------------------------------------------------------')
  70. disp('       poleg = roots(den)      % Computing the poles of the plant')
  71. disp('    ---------------------------------------------------------------')
  72. poleg  = roots(den)       
  73. disp(' ')
  74. disp(' ')
  75. disp(' ')
  76. disp('                                 (strike a key to continue ...)')
  77. pause
  78. clc
  79. disp(' ')
  80. disp('  ')
  81. disp('    Zeros of the Plant (minimum phase):')
  82. disp('  ')
  83. disp('    -------------------------------------------------------------------')
  84. disp('      tzerog = roots(num)     % Computing the zeros')
  85. disp('    -------------------------------------------------------------------')
  86. tzerog = roots(num(1,4:7)) 
  87. disp('  ')
  88. disp(' ')
  89. disp(' ')
  90. disp('                                 (strike a key to continue ...)')
  91. pause
  92. clc
  93. disp(' ')
  94. disp('         - - - Computing Nyquist plot of the open loop plant - - -')
  95. w = logspace(-4,5,200);
  96. lenw = length(w);
  97. [reg,img] = nyquist(ag,bg,cg,dg,1,w); 
  98. reg = [reg;reg(lenw:-1:1,:)];img = [img;-img(lenw:-1:1,:)];
  99. disp(' ')
  100. disp(' ')
  101. disp('                     (strike a key to see the Nyquist plot of G(s) ...)')
  102. pause
  103. clg
  104. plot(reg,img)
  105. title('Fighter Pitch Axis Open Loop')
  106. xlabel('real(G(s))')
  107. ylabel('imag(G(s))')
  108. grid
  109. pause
  110. clc
  111. disp('                      << Problem Formulation >>')
  112. disp(' ')
  113. disp('     The LQG/LTR procedure described here does the loop trnasfer revovery')
  114. disp('     at plant output:')
  115. disp(' ')
  116. disp('        Given a transfer function G(s), find a stabilizing controller')
  117. disp('        F(s) such that the "Kalman Filter Loop Transfer Fucntion" --')
  118. disp('                                     -1')
  119. disp('                       Lq(s) = C(Is-A) Kf')
  120. disp('        is recovered as the "control weighting" = Q replaced by')
  121. disp('                                 T')
  122. disp('              Q <------ Q + q*C*C   and "q" goes to infinity.')
  123. disp('   ')
  124. disp('     The Kalman filter loop gain reserves the following nice propertis')
  125. disp('     in each of the feedback loops:')
  126. disp('                   1). Infinity gain margin')
  127. disp('                   2). +- 60 deg. phase margin')
  128. disp('     However, LQG/LTR procedure works only if the plant is:')
  129. disp('              1). Mininum phase')
  130. disp('              2). Having more (or equal) no. of inputs than outputs')
  131. disp(' ')
  132. disp('  ')
  133. disp('                                 (strike a key to continue ...)')
  134. pause
  135. clc
  136. clc
  137. disp('  ')
  138. disp(' ')
  139. disp('                              << DESIGN PROCEDURE >>')
  140. disp('  ')
  141. disp('            * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *')
  142. disp('            *   [Step 1]. Select noise weightings Xi & Th such that   *')
  143. disp('            *             they achieve a desirable Kalman filter gain *')
  144. disp('            *             (e.g., Xi = BB`, Th = I, or anything better)*')
  145. disp('            *                                                         *')
  146. disp('            *   [Step 2]. Assign a set of recovery gain for Q         *')
  147. disp('            *             (e.g., q = [1, 1e5, 1e10, 1e15])            *')
  148. disp('            *                                                         *')
  149. disp('            *   [Step 3]. Run M-file LTRY.M with initial Q = C`C and  *')
  150. disp('            *             R = I, then the recovered Kalman gain will  *')
  151. disp('            *             be presented on the screen                  *')
  152. disp('            * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *')
  153. disp('  ')
  154. disp('  ')
  155. disp('                                 (strike a key to continue ...)')
  156. pause
  157. clc
  158. disp(' ')
  159. disp('                         << Start Computation >>')
  160. disp('     ------------------------------------------------------------------')
  161. disp('       Xi = bg*bg`; Th = 1;           % Assign noise weighting')
  162. disp('       q  = [1 1e5 1e10 1e15];        % Assign recovery gain for Q')
  163. disp('       Q = cg`*cg; R = 1;             % Assign initial control wt. ')
  164. disp('       XiTh = diagmx(Xi,Th)           % Put Xi & Th into compact form')
  165. disp('       [Kf] = lqrc(ag`,cg`,XiTh);     % Solve Kalman filter gain')
  166. disp('       Kf = Kf`                       % via the duallity of LQR')
  167. disp('       [re,im] = nyquist(ag,Kf,cg,0,1,w); % Compute Nyquist plot ')
  168. disp('       lenw = length(w);              % length of w')
  169. disp('       svk = [ [re;re(lenw:-1:1,:)] [im;-im(lenw:-1:1,:)] ];')
  170. disp('                                      % Pack frequency response')
  171. disp('     ------------------------------------------------------------------')
  172. Xi = bg*bg'; Th = 1;                 % Assign noise weighting
  173. q  = [1 1e5 1e10 1e15];              % Assign recovery gain for Q
  174. Q = cg'*cg; R = 1;                   % Assign initial control wt.
  175. XiTh = diagmx(Xi,Th);                % Put Xi & Th into compact form
  176. disp('   ')
  177. disp('     - - - Solving Riccati for the Kalman Filter Gain - - -')
  178. [Kf] = lqrc(ag',cg',XiTh);           % Solve Kalman filter gain
  179. Kf = Kf';                            % via the duallity of LQR
  180. disp(' ')
  181. disp('     - - - Computing Nyquist Plot of the Kalman Filter Gain - - -')
  182. [rek,imk] = nyquist(ag,Kf,cg,0,1,w);   % Compute Nyquist plot
  183. rek = [rek;rek(lenw:-1:1,:)]; imk = [imk;-imk(lenw:-1:1,:)];
  184. svk = [rek imk];
  185. disp(' ')
  186. disp(' ')
  187. disp('   (strike a key to see the Nyquist plot of the Kalman filter gain ...)')
  188. disp('(Note: 2 encirclements CW of -1 is due to the two unstable plant poles)')
  189. pause
  190. clg
  191. plot(rek,imk)
  192. title('Nyquist Plot of the Kalman Filter Gain')
  193. xlabel('real(L(s))')
  194. ylabel('imag(L(s))')
  195. grid
  196. pause
  197. clc
  198. disp('  ')
  199. disp('  ')
  200. disp('                  << LQG/LTR Procedure at Plant Output >>')
  201. disp('    ------------------------------------------------------------------')
  202. disp('       ss_g = mksys(ag,bg,cg,dg); % G(s) ---> system matrix')
  203. disp('       [ss_f,svl] = ltry(ss_g,Kf,Q,R,q,w,svk); % LQG/LTR at y')
  204. disp('       [af,bf,cf,df] = branch(ss_f);')
  205. disp('    ------------------------------------------------------------------')
  206. ss_g = mksys(ag,bg,cg,dg);
  207. [ss_f,svl] = ltry(ss_g,Kf,Q,R,q,w,svk); % LQG/LTR at y
  208. [af,bf,cf,df] = branch(ss_f);
  209. disp(' ')
  210. disp('  ')
  211. disp('                                 (strike a key to continue ...)')
  212. pause
  213. disp(' ')
  214. disp('  - - - Computing the SV of Controller - - -')
  215. svf = sigma(af,bf,cf,df,1,w); svf = 20*log10(svf);
  216. semilogx(w,svf)
  217. title('LQG/LTR Controller')
  218. xlabel('Rad/Sec')
  219. ylabel('SV (db)')
  220. pause
  221. clc
  222. format short e
  223. disp('   ')
  224. disp('  ')
  225. disp('   Poles of the controller:')
  226. polf = eig(af)
  227. disp(' ')
  228. disp('  ')
  229. disp('                                 (strike a key to continue ...)')
  230. pause
  231. clc
  232. disp('   ')
  233. disp('   Poles of the closed-loop TF:')
  234. [al,bl,cl,dl] = series(af,bf,cf,df,ag,bg,cg,dg);
  235. [acl,bcl,ccl,dcl] = feedbk(al,bl,cl,dl,2);
  236. polcl = eig(acl)
  237. disp(' ')
  238. disp(' ')
  239. disp('                                 (strike a key to continue ...)')
  240. pause
  241. %
  242. % ------- End of SLTRDEMO.M --- RYC/MGS %
  243.