home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 7.ddi / ROBUST.DI$ / LTRY.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  3.4 KB  |  112 lines
  1. function [af,bf,cf,df,svl] = ltry(Z1,Z2,Z3,Z4,Z5,Z6,Z7,Z8,Z9,Z10)
  2. % [ss_f,svl] = ltry(ss_,Kf,Q,R,q,w,svk) or
  3. % [af,bf,cf,df,svl] = ltry(A,B,C,D,Kf,Q,R,q,w,svk) produces 
  4. %    LQG/LTR at inputs of the plant, such that the LQG loop TF
  5. %    will converge to KBF's loop TF as the state wt. goes to INF:
  6. %                        -1                  -1
  7. %     GKc(Is-A+B*Kc+Kf*C) Kf -------> C(Is-A) Kf   (as q ---> INF)
  8. %                                 
  9. %  Inputs: sys -- system matrix, dim -- no.of states
  10. %          Kf -- Kalman filter gain, 
  11. %  (optional) svk(MIMO) -- SV of (C inv(Is-A)Kf)
  12. %  (optional) svk(SISO) -- [re im;re(reverse order) -im(reverse order)] 
  13. %                          of the complete Nyquist plot
  14. %          w -- frequency points
  15. %          Q -- state weighting, R -- control weighting
  16. %          q -- a row vector containing a set of recovery gains
  17. %               (nq: length of q)  
  18. %  Outputs: svl -- singular value plots of all the recovery points
  19. %           svl(SISO) -- Nyquist loci svl = [re(1:nq) im(1:nq)]
  20. %           final state-space controller (af,bf,cf,df)
  21. %
  22.  
  23. % R. Y. Chiang & M. G. Safonov 6/86
  24. % Copyright (c) 1988 by the MathWorks, Inc.
  25. % All Rights Reserved.
  26. % ---------------------------------------------------------------------
  27. %
  28. A=[];B=[];C=[];D=[];Kf=[];Q=[];R=[];q=[];w=[];svk=[];
  29. nag1 = nargin;
  30. inargs = '(A,B,C,D,Kf,Q,R,q,w,svk)';
  31. eval(mkargs(inargs,nag1,'ss'));
  32. nag1 = nargin; % NARGIN may have changed after the expansion
  33.  
  34. nq = length(q);
  35. [md,nd] = size(D);
  36. %
  37. if (md == 1) & (nd == 1)
  38.   for i = 1:nq
  39.     disp('  ')
  40.     disp('  ')
  41.     disp(['   LQG/LTR ---> recovery gain = ' num2str(q(1,i)) ' ..']);
  42.     Q = Q + q(1,i)*C'*C;
  43.     qrnc = diagmx(Q,R);
  44.     Kc = lqrc(A,B,qrnc);
  45.     af = A - B*Kc - Kf*C + Kf*D*Kc;
  46.     bf = Kf; cf = Kc; df = zeros(size(Kc)*[1;0],size(Kf)*[0;1]);
  47.     [al,bl,cl,dl] = series(af,bf,cf,df,A,B,C,D);
  48.     [re,im] = nyquist(al,bl,cl,dl,1,w); 
  49.     lenw = length(w);
  50.     re = [re;re(lenw:-1:1,:)]; im = [im;-im(lenw:-1:1,:)];
  51.     if i == 1
  52.           rel = re;          iml = im;
  53.     else
  54.           rel = [rel re];    iml = [iml im];
  55.     end
  56.     disp(' ')
  57.     disp('(strike a key to see the plots .... hit <RET> again to continue)')
  58.     pause
  59.     if nag1 == 10
  60.        plot(svk(:,1),svk(:,2),rel,iml)
  61.     else
  62.        plot(rel,iml)
  63.     end
  64.     title(['NYQUIST LOCI -- LQG/LTR (recov. gain ---> ' num2str(q(1,i)) ')'])
  65.     xlabel('REAL')
  66.     ylabel('IMAG')
  67.     grid
  68.     pause
  69.   end
  70.   svl = [rel iml];
  71. end
  72. %
  73. if (md > 1) | (nd > 1)
  74.   for i = 1:nq
  75.     disp('  ')
  76.     disp('  ')
  77.     disp(['   LQG/LTR ---> recovery gain = ' num2str(q(1,i)) ' ..']);
  78.     Q = Q + q(1,i)*C'*C;
  79.     qrnc = diagmx(Q,R);
  80.     Kc = lqrc(A,B,qrnc);
  81.     af = A - B*Kc - Kf*C + Kf*D*Kc;
  82.     bf = Kf; cf = Kc; df = zeros(size(Kc)*[1;0],size(Kf)*[0;1]);
  83.     [al,bl,cl,dl] = series(af,bf,cf,df,A,B,C,D);
  84.     sv = sigma(al,bl,cl,dl,1,w); sv = 20*log10(sv);
  85.     if i == 1
  86.           svl = sv;
  87.     else
  88.           svl = [svl;sv];
  89.     end
  90.     disp(' ')
  91.     disp('(strike a key to see the plots ..... hit <RET> again to continue)')
  92.     pause
  93.     if nag1 == 10
  94.       semilogx(w,svk,w,svl)
  95.     else
  96.       semilogx(w,svl)
  97.     end
  98.     title(['SV BODE PLOT --- LQG/LTR (recov. gain --->' num2str(q(1,i)) ')'])
  99.     xlabel('Frequency - Rad/Sec')
  100.     ylabel('SV - db')
  101.     grid
  102.     pause
  103.   end
  104. end
  105. %
  106. if xsflag 
  107.    af = mksys(af,bf,cf,df);
  108.    bf = svl;
  109. end
  110. %
  111. % ------- End of LTRY.M -- RYC %
  112.