home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 7.ddi / ROBUST.DI$ / HINFLIM.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  10.3 KB  |  310 lines
  1. %
  2. %     << 4-block L-inf optimal Controller (all solution formulae) >>
  3. %           (derived by Limebeer and Kasenally, Dec. 1987)
  4. %
  5. %   HINFLIM is a script file which computes the H-inf optimal controller 
  6. %   using L-K'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 Limebeer and Kasenally) - - -')
  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 the D22 term:
  51. %
  52. [mD22,nD22] = size(D22);
  53. d22 = zeros(mD22,nD22);
  54. %
  55. % ------------------------------------------- 2) Find scaling factors:
  56. %
  57. [p1,m2] = size(D12);
  58. [uu,sig12,vv] = svd(D12);
  59. u1 = uu(:,1:m2);
  60. u2 = uu(:,m2+1:p1);
  61. su = vv*inv(sig12(1:m2,1:m2));
  62. sz = [u2';u1'];
  63. %
  64. [p2,m1] = size(D21);
  65. [uuu,sig21,vvv] = svd(D21);
  66. v1 = vvv(:,1:p2);
  67. v2 = vvv(:,p2+1:m1);
  68. sy = inv(sig21(1:p2,1:p2))*uuu';
  69. sw = [v2 v1];
  70. %
  71. b1 = B1*sw;
  72. c1 = sz*C1;
  73. b2 = B2*su;
  74. c2 = sy*C2;  
  75. d11 = sz*D11*sw;
  76. d12 = sz*D12*su;
  77. d21 = sy*D21*sw;
  78. %
  79. % ----------------------------------- 3) Checking the frequency @ inf:
  80. %
  81. d1111 = d11(1:p1-m2,1:m1-p2);
  82. d1121 = d11(p1-m2+1:p1,1:m1-p2);
  83. d1112 = d11(1:p1-m2,m1-p2+1:m1);
  84. d1122 = d11(p1-m2+1:p1,m1-p2+1:m1);
  85. mxsvd11r = max(svd([d1111 d1112]));
  86. mxsvd11c = max(svd([d1111;d1121]));
  87. if mxsvd11r >= 1 | mxsvd11c >= 1
  88.      disp('  ')
  89.      disp('    ------------------------------------------------------------');
  90.      disp('       NO CONTROLLER MEETS THE SPEC. EVEN AT FREQ. INF ALONE !!')
  91.      disp('       ADJUST "Gam" AND TRY AGAIN ...')
  92.      disp('    ------------------------------------------------------------');
  93.      break
  94. end
  95. %
  96. % ----------------------------------- 4) Orthogonal complements of D12 & D21:
  97. %
  98. d12p = ortc(d12);
  99. d21p = ortr(d21);
  100. % 
  101. % ----------------------------------------------------------------------------
  102. % 
  103. % ---------------------------------------- Start to compute the controller:
  104. siz = [1 0]';
  105. n = size(A)*siz;
  106. gam = 1;                    % for small-gain problem
  107. gam2 = gam*gam;
  108. RI = gam2*eye(n);
  109. %
  110. % ---------------------------------------- Fix D11 term:
  111. %
  112. if m1 == p2 | p1 == m2
  113.    fopt = -d1122;
  114. else
  115.    fopt  = -(d1122+d1121*inv(eye(m1-p2)-d1111'*d1111)*d1111'*d1112);
  116. end
  117. %
  118. a   = A  + b2*fopt*c2;
  119. b1  = b1 + b2*fopt*d21;
  120. c1  = c1 + d12*fopt*c2;
  121. d11 = d11+ d12*fopt*d21;
  122. %                                                  ~
  123. % ---------------------------------------- Solving P Riccati:
  124. %
  125. ar1 = a+(b1*d11'*d12p*d12p' - b2*d12'*gam2)*...
  126.       inv(gam2*eye(size(d11)*siz)-d11*d11'*d12p*d12p')*c1;
  127. q1 = c1'*d12p*inv(gam2*eye(size(d12p')*siz)-...
  128.      d12p'*d11*d11'*d12p)*d12p'*c1;
  129. r1 = RI*b2*b2'-(b1-b2*d12'*d11)*gam2*inv(gam2*eye(size(d11')*siz)-...
  130.      d11'*d12p*d12p'*d11)*(b1'-d11'*d12*b2');
  131. disp(' ');
  132. disp(' - - - Solving the 1st Riccati - - -');
  133. if AREFLAG > 0
  134.    [P1,P2,lamPc,Perr,wellposed,P] = aresolv(ar1,q1,r1,aretype);
  135. else
  136.    [P1,P2,lamPc,Perr,wellposed,P] = aresolv(ar1,q1,r1);
  137. end
  138. %                                                  ~
  139. % ---------------------------------------- Solving S Riccati:
  140. %
  141. ar2 = a+b1*inv(gam2*eye(size(d21p')*siz)-d21p'*d21p*d11'*d11)*...
  142.       (d21p'*d21p*d11'*c1-gam2*d21'*c2);
  143. q2 = b1*d21p'*inv(gam2*eye(size(d21p)*siz)-...
  144.      d21p*d11'*d11*d21p')*d21p*b1';
  145. r2 = RI*c2'*c2-(c1'-c2'*d21*d11')*gam2*inv(gam2*eye(size(d11)*siz)-...
  146.      d11*d21p'*d21p*d11')*(c1-d11*d21'*c2);
  147. disp('   ');
  148. disp(' - - - Solving the 2nd Riccati - - -');
  149. if AREFLAG > 0
  150.    [S1,S2,lamSc,Serr,wellposed,S] = aresolv(ar2',q2,r2,aretype);
  151. else
  152.    [S1,S2,lamSc,Serr,wellposed,S] = aresolv(ar2',q2,r2);
  153. end
  154. lamP  = min(real(eig(P)));
  155. lamS  = min(real(eig(S)));
  156. format long
  157. lamPS = max(real(eig(P*S)))
  158. if (lamP<0 & abs(lamP)>1e-10) | (lamS<0 & abs(lamS)>1e-10) | lamPS > 1
  159.          disp('  ')
  160.          disp('    ----------------------------------------------------');
  161.          disp('       NO STABILIZING CONTROLLER MEETS THE SPEC. !!')
  162.          disp('       ADJUST "Gam" AND TRY AGAIN ...')
  163.          disp('    ----------------------------------------------------');
  164.          break
  165. end
  166. %
  167. % ---------------------------- Controller in descriptor form:
  168. %
  169. ek = eye(n)-S*P*RI;
  170. bk1 = -gam2*(b1+S*c1'*d11 -S*c2'*d21*...
  171.       (d11'*d11-gam2*eye(size(d11')*siz)))...
  172.       *inv(gam2*eye(size(d21p')*siz)-...
  173.       d21p'*d21p*d11'*d11)*d21';
  174. ck1 = -gam2*d12'*inv(gam2*eye(size(d11)*siz)...
  175.       -d11*d11'*d12p*d12p')...
  176.       *(c1+d11*b1'*P-(d11*d11'-...
  177.       gam2*eye(size(d11)*siz))*d12*b2'*P);
  178. dk11 = zeros(size(ck1)*siz,size(bk1)*[0 1]');
  179. %
  180. bk2 = b1*d21'*d22-b2+(gam2*S*c1'+b1*d21p'*d21p*d11'-...
  181.          gam2*S*c2'*d21*d11')*...
  182.          inv(gam2*eye(size(d11)*siz)-...
  183.          d11*d21p'*d21p*d11')*(d11*d21'*d22-d12)+...
  184.          gam2*S*c2'*d22;
  185. rbk2 = chol(d12'*(gam2*eye(size(d11)*siz)-d11*d11')...
  186.          *gam2*inv(gam2*eye(size(d12p)*siz)-...
  187.          d12p*d12p'*d11*d11')*d12);
  188. rbk2 = rbk2';
  189. bk2 = bk2*rbk2;
  190. ck2 = (d22*d12'*c1-c2)+d22*b2'*P*gam2+(d22*d12'*d11-d21)...
  191.          *inv(gam2*eye(size(d11')*siz)-d11'*d12p*d12p'*d11)*...
  192.          (d11'*d12p*d12p'*c1-d11'*d12*b2'*P*gam2+b1'*P*gam2);
  193. rck2 = chol(gam2*d21*inv(gam2*eye(size(d11')*siz)-...
  194.          d11'*d11*d21p'*d21p)*...
  195.          (gam2*eye(size(d11')*siz)-d11'*d11)*d21');
  196. ck2 = rck2*ck2;
  197. dk12 = chol(d12'*(gam2*eye(size(d11)*siz)-d11*d11')*...
  198.            gam2*inv(gam2*eye(size(d12p)*siz)...
  199.           -d12p*d12p'*d11*d11')*d12);
  200. dk12 = -dk12';
  201. dk21 = chol(gam2*d21*inv(gam2*eye(size(d11')*siz)-...
  202.             d11'*d11*d21p'*d21p)*...
  203.            (gam2*eye(size(d11')*siz)-d11'*d11)*d21');
  204. dk21 = -dk21;
  205. dk22 = dk21*d22*dk12+inv(dk21')*d21*d11'*...
  206.           inv(gam2*eye(size(d11)*siz)-...
  207.           d11*d21p'*d21p*d11')*d12*dk12*gam2;
  208. %
  209. ak = -(S*c1'*d11+b1)*inv(gam2*eye(size(d21p')*siz)-...
  210.       d21p'*d21p*d11'*d11)...
  211.      *d21'*d21*inv(gam2*eye(size(d11')*siz)-...
  212.      d11'*d12p*d12p'*d11);
  213. ak = ak*(d11'*d12p*d12p'*c1*gam2+gam2*gam2*b1'*P-...
  214.      gam2*gam2*d11'*d12*b2'*P...
  215.      +gam2*(gam2*eye(size(d11')*siz)-...
  216.      d11'*d12p*d12p'*d11)*d21'*c2);
  217. ak0 = -S*c2'*d21*inv(gam2*eye(size(d11')*siz)-...
  218.       d11'*d11*d21p'*d21p)*...
  219.       (gam2*eye(size(d11')*siz)-d11'*d11)*...
  220.       inv(gam2*eye(size(d11')*siz)-d11'*d12p*d12p'*d11);
  221. ak0 = ak0*(d11'*d12p*d12p'*c1*gam2+gam2*gam2*b1'*P-...
  222.        gam2*gam2*d11'*d12*b2'*P +...
  223.        gam2*(gam2*eye(size(d11')*siz)-...
  224.        d11'*d12p*d12p'*d11)*d21'*c2);
  225. ak = ak+ak0;
  226. ak = ak+ek*(a+(b1*d11'*d12p*d12p'-b2*d12'*gam2)*...
  227.      inv(gam2*eye(size(d11)*siz)-...
  228.      d11*d11'*d12p*d12p')*c1-(gam2*b2*b2'-...
  229.      (b1-b2*d12'*d11)*gam2*inv(gam2*eye(size(d11')*siz)-...
  230.      d11'*d12p*d12p'*d11)*(b1'-d11'*d12*b2'))*P);
  231. ak = ak+bk1*d22*ck1;
  232. %
  233. % --------------- Flip the sign of controller:
  234. %
  235. bk1 = -bk1; bk2 = -bk2;
  236. dk11 = -dk11; dk12 = -dk12; dk21 = -dk21; dk22 = -dk22;
  237. %
  238. % ------------------------------------ Post-process the controller:
  239. %
  240. % ------------------------------------ 1) Reverse the "fopt" term:
  241. %
  242. dk11 = dk11 + fopt;
  243. %
  244. % ------------------------------------ 2) Reverse the controller scaling:
  245. %
  246. bk1  = bk1*sy;
  247. ck1  = su*ck1;
  248. dk11 = su*dk11*sy;
  249. dk12 = su*dk12;
  250. dk21 = dk21*sy;
  251. %
  252. % ------------------------------------ 3) Shifting D22 term:
  253. %
  254. temp = inv(eye(size(dk11)*[1;0]) + dk11*D22);
  255. ak   = ak-bk1*D22*temp*ck1;
  256. bk2  = bk2-bk1*D22*temp*dk12;
  257. ck2  = ck2-dk21*D22*temp*ck1;
  258. ck1  = temp*ck1;
  259. dk22 = dk22-dk21*D22*temp*dk12;
  260. dk12 = temp*dk12;
  261. dk11 = temp*dk11;
  262. temp = eye(size(D22)*[1;0])-D22*dk11;
  263. bk1  = bk1*temp;
  264. dk21 = dk21*temp;
  265. %
  266. % ----------------  Putting descriptor controller K(s) in state space form:
  267. %
  268. se = svd(ek);
  269. nullE = size(find(se<1.e-7))*[1;0];
  270. [ak,bk,ck,dk] = des2ss(ak,[bk1,bk2],[ck1;ck2],...
  271.                          [dk11,dk12;dk21,dk22],ek,nullE);
  272. [rbk1,cbk1] = size(bk1);
  273. [rbk2,cbk2] = size(bk2);
  274. [rck1,cck1] = size(ck1);
  275. [rck2,cck2] = size(ck2);
  276. bk1=bk(:,1:cbk1);
  277. bk2=bk(:,cbk1+1:cbk1+cbk2);
  278. ck1=ck(1:rck1,:);
  279. ck2=ck(rck1+1:rck1+rck2,:);
  280. dk11=dk(1:rck1,1:cbk1);
  281. dk12=dk(1:rck1,cbk1+1:cbk1+cbk2);
  282. dk21=dk(rck1+1:rck1+rck2,1:cbk1);
  283. dk22=dk(rck1+1:rck1+rck2,cbk1+1:cbk1+cbk2);
  284. %
  285. % --------------------- computing controller F(s) = (acp,bcp,ccp,dcp):
  286. %
  287. sysk = [ak,bk1,bk2;ck1,dk11,dk12;ck2,dk21,dk22];
  288. dimk = [size(ak)*[1 0]',size(bk1)*[0 1]',size(bk2)*[0 1]',...
  289.             size(ck1)*[1 0]',size(ck2)*[1 0]'];
  290. if ~( exist('AU') & exist('BU') & exist('CU') & exist('DU'))
  291.    acp = ak; bcp = bk1; ccp = ck1; dcp = dk11;
  292. else
  293.    [acp,bcp,ccp,dcp] = lftf(sysk,dimk,AU,BU,CU,DU);
  294. end
  295. %
  296. % ----------------------------------------------------------------------------
  297. %
  298. % ------------------------------------ Closed-loop TF (Ty1u1):
  299. %
  300. sysp = [A B1 B2;C1 D11 D12;C2 D21 D22];
  301. dimp = [n,size(B1)*[0 1]',size(B2)*[0 1]',...
  302.        size(C1)*[1 0]',size(C2)*[1 0]'];
  303. [acl,bcl,ccl,dcl] = lftf(sysp,dimp,acp,bcp,ccp,dcp);
  304. %
  305. clear sysp, clear dimp, clear a, clear b1, clear b2, clear c1, clear c2
  306. clear d11, clear d12, clear d21, clear d22, clear syscp, clear dimcp
  307. clear temp
  308. %
  309. % ---------- End of HINFLIM.M --- RYC/MGS -- %
  310.