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