www.gusucode.com > rctobsolete 工具箱 matlab源码程序 > rctobsolete/robust/ltry.m

    function [af,bf,cf,df,svl] = ltry(varargin)
%LTRY Continuous LQG/LTR control synthesis (at plant output).
%
% [ss_f,svl] = ltry(ss_,Kf,Q,R,q,w,svk) or
% [af,bf,cf,df,svl] = ltry(A,B,C,D,Kf,Q,R,q,w,svk) produces
%    LQG/LTR at inputs of the plant, such that the LQG loop TF
%    will converge to KBF's loop TF as the state wt. goes to INF:
%                        -1                  -1
%     GKc(Is-A+B*Kc+Kf*C) Kf -------> C(Is-A) Kf   (as q ---> INF)
%
%  Inputs: (A,B,C,D) -- system, or ss_ -- system matrix (built by "mksys")
%          Kf -- Kalman filter gain
%  (optional) svk(MIMO) -- SV of (C inv(Is-A)Kf)
%  (optional) svk(SISO) -- [re im;re(reverse order) -im(reverse order)]
%                          of the complete Nyquist plot
%          w -- frequency points
%          Q -- state weighting, R -- control weighting
%          q -- a row vector containing a set of recovery gains
%               (nq: length of q). At each iteration, Q <-- Q + q*C'*C;
%  Outputs: svl -- singular value plots of all the recovery points
%           svl(SISO) -- Nyquist loci svl = [re(1:nq) im(1:nq)]
%           final state-space controller (af,bf,cf,df)
%

% R. Y. Chiang & M. G. Safonov 6/86
% Copyright 1988-2004 The MathWorks, Inc.
% All Rights Reserved.
% ---------------------------------------------------------------------
%
A=[];B=[];C=[];D=[];Kf=[];Q=[];R=[];q=[];w=[];svk=[];
nag1 = nargin;
[emsg,nag1,xsflag,Ts,A,B,C,D,Kf,Q,R,q,w,svk]=mkargs5x('ss',varargin); error(emsg);
if Ts, error('LTI inputs must be continuous time (Ts=0)'), end
    % NAG1 may have changed after the expansion

nq = length(q);
[md,nd] = size(D);
%
if (md == 1) & (nd == 1)
  for i = 1:nq
    disp('  ')
    disp('  ')
    disp(['   LQG/LTR ---> recovery gain = ' num2str(q(1,i)) ' ..']);
    Q = Q + q(1,i)*C'*C;
    qrnc = diagmx(Q,R);
    Kc = lqrc(A,B,qrnc);
    af = A - B*Kc - Kf*C + Kf*D*Kc;
    bf = Kf; cf = Kc; df = zeros(size(Kc)*[1;0],size(Kf)*[0;1]);
    [al,bl,cl,dl] = series(af,bf,cf,df,A,B,C,D);
    [re,im] = nyquist(al,bl,cl,dl,1,w);
    lenw = length(w);
    re = [re;re(lenw:-1:1,:)]; im = [im;-im(lenw:-1:1,:)];
    if i == 1
          rel = re;          iml = im;
    else
          rel = [rel re];    iml = [iml im];
    end
    disp(' ')
    disp('(strike a key to see the plots .... hit <RET> again to continue)')
    pause
    if nag1 == 10
       plot(svk(:,1),svk(:,2),rel,iml)
    else
       plot(rel,iml)
    end
    title(['NYQUIST LOCI -- LQG/LTR (recov. gain ---> ' num2str(q(1,i)) ')'])
    xlabel('REAL')
    ylabel('IMAG')
    grid on
    pause
  end
  svl = [rel iml];
end
%
if (md > 1) | (nd > 1)
  for i = 1:nq
    disp('  ')
    disp('  ')
    disp(['   LQG/LTR ---> recovery gain = ' num2str(q(1,i)) ' ..']);
    Q = Q + q(1,i)*C'*C;
    qrnc = diagmx(Q,R);
    Kc = lqrc(A,B,qrnc);
    af = A - B*Kc - Kf*C + Kf*D*Kc;
    bf = Kf; cf = Kc; df = zeros(size(Kc)*[1;0],size(Kf)*[0;1]);
    [al,bl,cl,dl] = series(af,bf,cf,df,A,B,C,D);
    sv = sigma(al,bl,cl,dl,1,w); sv = 20*log10(sv);
    if i == 1
          svl = sv;
    else
          svl = [svl;sv];
    end
    disp(' ')
    disp('(strike a key to see the plots ..... hit <RET> again to continue)')
    pause
    if nag1 == 10
      semilogx(w,svk,w,svl)
    else
      semilogx(w,svl)
    end
    title(['SV BODE PLOT --- LQG/LTR (recov. gain --->' num2str(q(1,i)) ')'])
    xlabel('Frequency - Rad/Sec')
    ylabel('SV - db')
    grid on
    pause
  end
end
%
if xsflag
   af = mksys(af,bf,cf,df);
   bf = svl;
end
%
% ------- End of LTRY.M -- RYC %