home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 7.ddi / ROBUST.DI$ / HINFKGJD.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  7.8 KB  |  261 lines
  1. %
  2. %     << 4-block L-inf optimal Controller (all solution formulae) >>
  3. %               (derived by K. Glover & J. Doyle, 1988)
  4. %
  5. %   HINFKGJD is a script file which computes the H-inf optimal controller 
  6. %   using G-D's all-solution "two-Riccati" formulae.
  7. %   A particular controller F(s) is returned by default, however the
  8. %   other solution can be generated by prespecifing the transfer function
  9. %   in the workspace
  10. %                       U(s):= (AU,BU,CU,DU)
  11. %   where U(s) is a free stable contraction which parameterizes all the 
  12. %   stabilizing controllers.
  13. %
  14. %   Input data (must be in Upper Case !!!): 
  15. %              augmented plant (A,B1,B2,C1,C2,D11,D12,D21,D22) 
  16. %              stable contraction U(s):= (AU,BU,CU,DU) (optional)
  17. %   Output data: H-inf controller F(s):= (acp,bcp,ccp,dcp),
  18. %                CLTF Ty1u1:= (acl,bcl,ccl,dcl).
  19. %
  20.  
  21. % R. Y. Chiang & M. G. Safonov 2/10/88
  22. % Copyright (c) 1988 by the MathWorks, Inc.
  23. % All Rights Reserved.
  24. % ------------------------------------------------------------------------
  25. disp('   ')
  26. disp('                << H-inf Optimal Control Synthesis >>')
  27. disp(' ')
  28. disp(' - - - Computing 4-block L-inf optimal controller ')
  29. disp('       (using the all-solution formulae of Glover and Doyle) - - -')
  30. %
  31. flagcase1 = exist('b1');
  32. flagcase2 = exist('B1');
  33. if (flagcase1 > 0) & (flagcase2 < 1)
  34.  error('   THIS SCRIPT FILE REQUIRES THE INPUT VARIABLE NAMES IN UPPER CASE !!!')
  35. end
  36. %
  37. FLAG = exist('AU');
  38. if FLAG < 1
  39.    disp('  ')
  40.    disp(' - - - Solving a particular solution F(s) (U(s) = 0 case) - - -');
  41. end
  42. if FLAG > 0
  43.    disp('  ')
  44.    disp(' - - - Solving the all-solution F(s) (U(s):=(AU,BU,CU,DU)) - - -');
  45. end
  46. AREFLAG = exist('aretype');
  47. %
  48. % ------------------------------------------- Pre-process the augmented plant:
  49. %
  50. % ------------------------------------------- 1) Zero out D22 term:
  51. %
  52. [mD22,nD22] = size(D22);
  53. d22 = zeros(mD22,nD22);
  54. %
  55. % ------------------------------------------- 2) Find the scaling factors:
  56. %
  57. [n,n] = size(A);
  58. [p1,m2] = size(D12);
  59. [uu,sig12,vv] = svd(D12);
  60. u1 = uu(:,1:m2);
  61. u2 = uu(:,m2+1:p1);
  62. su = vv*inv(sig12(1:m2,1:m2));
  63. sz = [u2';u1'];
  64. %
  65. [p2,m1] = size(D21);
  66. [uuu,sig21,vvv] = svd(D21);
  67. v1 = vvv(:,1:p2);
  68. v2 = vvv(:,p2+1:m1);
  69. sy = inv(sig21(1:p2,1:p2))*uuu';
  70. sw = [v2 v1];
  71. %
  72. b1  = B1*sw;
  73. b2  = B2*su;
  74. c1  = sz*C1;
  75. c2  = sy*C2;
  76. d11 = sz*D11*sw;
  77. d21 = sy*D21*sw;
  78. d12 = sz*D12*su;
  79. %
  80. % ----------------------------------------- 3) Check frequency @ infinity:
  81. %
  82. d1111 = d11(1:p1-m2,1:m1-p2);
  83. d1121 = d11(p1-m2+1:p1,1:m1-p2);
  84. d1112 = d11(1:p1-m2,m1-p2+1:m1);
  85. d1122 = d11(p1-m2+1:p1,m1-p2+1:m1);
  86. mxsvd11r = max(svd([d1111 d1112]));
  87. mxsvd11c = max(svd([d1111;d1121]));
  88. if mxsvd11r >= 1 | mxsvd11c >= 1
  89.      disp('  ')
  90.      disp('    ------------------------------------------------------------');
  91.      disp('       NO CONTROLLER MEETS THE SPEC. EVEN AT FREQ. INF ALONE !!')
  92.      disp('       ADJUST "Gam" AND TRY AGAIN ...')
  93.      disp('    ------------------------------------------------------------');
  94.      break
  95. end
  96. %
  97. % ----------------------------------------------------------------------------
  98. %
  99. % ----------------------------------------- Start to compute the controller:
  100. gam = 1;
  101. gam2 = gam*gam;
  102. %
  103. % ----------------------------------------- Fixing D11 matrix:
  104. %
  105. if m1 == p2 | p1 == m2
  106.    dk11 = -d1122;
  107.    dk12 = eye(m2);
  108.    dk21 = eye(p2);
  109. else
  110.    gamd1  = gam2*eye(p1-m2)-d1111*d1111';
  111.    gamd2  = gam2*eye(m1-p2)-d1111'*d1111;
  112.    dk11   = -d1121*d1111'*inv(gamd1)*d1112-d1122;
  113.    dk1212 = eye(m2) - d1121*inv(gamd2)*d1121';
  114.    dk12   = chol(dk1212); dk12 = dk12';
  115.    dk2121 = eye(p2) - d1112'*inv(gamd1)*d1112;
  116.    dk21   = chol(dk2121);
  117. end
  118. %                                                 
  119. % ---------------------------------------- Solving X Riccati:
  120. %
  121. Zr = zeros(m1+m2,m1+m2);
  122. Zr(1:m1,1:m1) = gam2*eye(m1);
  123. d1d = [d11 d12];
  124. R = d1d'*d1d - Zr;
  125. Ri = inv(R); 
  126. B = [b1 b2];
  127. ax = A-B*Ri*d1d'*c1;
  128. rx = B*Ri*B';
  129. qx = c1'*c1 - c1'*d1d*Ri*d1d'*c1;
  130. %
  131. disp(' ');
  132. disp(' - - - Solving the 1st Riccati - - -');
  133. if AREFLAG > 0
  134.    [X1,X2,lamXc,Xerr,wellposed,X] = aresolv(ax,qx,rx,aretype);
  135. else
  136.    [X1,X2,lamXc,Xerr,wellposed,X] = aresolv(ax,qx,rx);
  137. end
  138. %                                                  
  139. % ---------------------------------------- Solving Y Riccati:
  140. %
  141. Zrw = zeros(p1+p2,p1+p2);
  142. Zrw(1:p1,1:p1) = gam2*eye(p1);
  143. dd1 = [d11;d21];
  144. Rw = dd1*dd1' - Zrw;
  145. Rwi = inv(Rw);
  146. C = [c1;c2];
  147. ay = A' - C'*Rwi*dd1*b1';
  148. ry = C'*Rwi*C;
  149. qy = b1*b1' - b1*dd1'*Rwi*dd1*b1';
  150. disp('   ');
  151. disp(' - - - Solving the 2nd Riccati - - -');
  152. if AREFLAG > 0
  153.    [Y1,Y2,lamYc,Yerr,wellposed,Y] = aresolv(ay,qy,ry,aretype);
  154. else
  155.    [Y1,Y2,lamYc,Yerr,wellposed,Y] = aresolv(ay,qy,ry);
  156. end
  157. lamX = min(real(eig(X)));
  158. lamY = min(real(eig(Y)));
  159. format long
  160. lamXY = max(real(eig(X*Y)))
  161. if (lamX<0 & abs(lamX)>1e-10) | (lamY<0 & abs(lamY)>1e-10) | lamXY > 1
  162.          disp('  ')
  163.          disp('    ----------------------------------------------------');
  164.          disp('       NO STABILIZING CONTROLLER MEETS THE SPEC. !!')
  165.          disp('       ADJUST "Gam" AND TRY AGAIN ...')
  166.          disp('    ----------------------------------------------------');
  167.          break
  168. end
  169. %
  170. % ---------------------------- Constant Matrices:
  171. %
  172. F   = - Ri*(d1d'*c1+B'*X);
  173. F11 = F(1:m1-p2,:);
  174. F12 = F(m1-p2+1:m1,:);
  175. F2  = F(m1+1:m1+m2,:);
  176. %
  177. H   = - (b1*dd1'+Y*C')*Rwi;
  178. H11 = H(:,1:p1-m2);
  179. H12 = H(:,p1-m2+1:p1);
  180. H2  = H(:,p1+1:p1+p2);
  181. %
  182. % ---------------------------- Controller in Descriptor form:
  183. %
  184. Z   = eye(n) - inv(gam2)*Y*X;
  185. %
  186. bk2    = (b2+H12)*dk12;
  187. ck2    = -dk21*(c2+F12);
  188. bk1    = -H2+bk2*inv(dk12)*dk11;
  189. ck1    = F2+dk11*inv(dk21)*ck2;
  190. ak     = A*Z + H*C*Z + bk2*inv(dk12)*ck1;
  191. %
  192. % ------------------------------------- Post-process the controller:
  193. %
  194. % ------------------------------------- 1) Reverse the controller scaling:
  195. %
  196. bk1 = bk1*sy;
  197. ck1 = su*ck1;
  198. dk11 = su*dk11*sy;
  199. dk12 = su*dk12;
  200. dk21 = dk21*sy;
  201. %
  202. % ------------------------------------- 2) Shifting D22 term:
  203. %
  204. temp = inv(eye(size(dk11)*[1;0]) + dk11*D22);
  205. ak   = ak-bk1*D22*temp*ck1;
  206. bk2  = bk2-bk1*D22*temp*dk12;
  207. ck2  = ck2-dk21*D22*temp*ck1;
  208. ck1  = temp*ck1;
  209. dk22 = dk22-dk21*D22*temp*dk12;
  210. dk12 = temp*dk12;
  211. dk11 = temp*dk11;
  212. temp = eye(size(D22)*[1;0])-D22*dk11;
  213. bk1  = bk1*temp;
  214. dk21 = dk21*temp;
  215. dk22 = zeros(size(dk21)*[1;0],size(dk12)*[0;1]);
  216. %
  217. % ----------------  Putting descriptor controller K(s) in state space form:
  218. %
  219. se = svd(Z);
  220. nullZ = size(find(se<1.e-7))*[1;0];
  221. [ak,bk,ck,dk] = des2ss(ak,[bk1,bk2],[ck1;ck2],...
  222.                          [dk11,dk12;dk21,dk22],Z,nullZ);
  223. [rbk1,cbk1] = size(bk1);
  224. [rbk2,cbk2] = size(bk2);
  225. [rck1,cck1] = size(ck1);
  226. [rck2,cck2] = size(ck2);
  227. bk1=bk(:,1:cbk1);
  228. bk2=bk(:,cbk1+1:cbk1+cbk2);
  229. ck1=ck(1:rck1,:);
  230. ck2=ck(rck1+1:rck1+rck2,:);
  231. dk11=dk(1:rck1,1:cbk1);
  232. dk12=dk(1:rck1,cbk1+1:cbk1+cbk2);
  233. dk21=dk(rck1+1:rck1+rck2,1:cbk1);
  234. dk22=dk(rck1+1:rck1+rck2,cbk1+1:cbk1+cbk2);
  235. %
  236. % --------------------- computing controller F(s) = (acp,bcp,ccp,dcp):
  237. %
  238. sysk = [ak,bk1,bk2;ck1,dk11,dk12;ck2,dk21,dk22];
  239. dimk = [size(ak)*[1 0]',size(bk1)*[0 1]',size(bk2)*[0 1]',...
  240.             size(ck1)*[1 0]',size(ck2)*[1 0]'];
  241. if ~( exist('AU') & exist('BU') & exist('CU') & exist('DU'))
  242.    acp = ak; bcp = bk1; ccp = ck1; dcp = dk11;
  243. else
  244.    [acp,bcp,ccp,dcp] = lftf(sysk,dimk,AU,BU,CU,DU);
  245. end
  246. %
  247. % ---------------------------------------------------------------------------
  248. %
  249. % ------------------------------------ Closed-loop TF (Ty1u1):
  250. %
  251. sysp = [A B1 B2;C1 D11 D12;C2 D21 D22];
  252. dimp = [n,size(B1)*[0 1]',size(B2)*[0 1]',...
  253.        size(C1)*[1 0]',size(C2)*[1 0]'];
  254. [acl,bcl,ccl,dcl] = lftf(sysp,dimp,acp,bcp,ccp,dcp);
  255. %
  256. clear sysp, clear dimp, clear a, clear b1, clear b2, clear c1, clear c2
  257. clear d11, clear d12, clear d21, clear d22, clear syscp, clear dimcp
  258. clear temp
  259. %
  260. % ---------- End of HINFKGJD.M --- RYC/MGS -- %
  261.