lmi1b.m

线性时变系统控制器设计的工具包

% lmi1b.m
%
% solve the first set of LMIs of GNC paper
% 
% solve for constant Dc and freq. dependant Dw1 that minimize
%
%   || diag(Dc,Dw1_i)*M(jw_i)*diag(Dc,Dw1_i)^-1 ||inf   over all i
%
% this gives the (sub)optimal scale Dc, if # of freq. pts = inf would be optimal
%
% Dc is used to scale the TV uncertainties
% Dw2 is used to scale the LTI uncertainties
% -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
%
%  usage:  [Dc,Dw1,ubnd1,ubnd1_pk,xopt1,x_init2,cpu_time] = ...
%                      lmi1b(M,w,struct1,struct2,solv_options,gam_init,x_init);
% 
%
% INPUTS: ----------
%
%	M - system to be analyzed (system matrix)
%      NOTE: currently, assume M has equal number of in/outputs
%            If it is not you'll have to add some dummy I/Os
%
%  w - vector of frequency points to be computed for, the solution
%		 to this set of LMIs (the 1st set) takes a while, thus you
%		 may want to use a few freq. points and then include more in 
%      the next step. It helps to look at the freq. response or msv
%      plot to help select the freq points to use, making sure to 
%      include "trouble spots" and a few others spread across all
%      frequencies of interest
%
%  struct1 - structure of the TV uncertainty
%         The rows of struct1 are given as:
%            [n 1] for parameters repeated n times
%            [n 0] for n by n blocks
%
%  struct2 - structure of the LTI uncertainty
%          The rows of struct2 are defined the same as struct1
%
%  NOTE: blk2strt.m can be used to convert a blk description used for mu.m
%        into a struct description used here
%
% OPTIONAL INPUTS: -------------
%
%  solv_options - is a 1x5 vector that specifies options for LMI solvers
%
%     NOTE: solv_options=[] or notdefined gives the default values
%           to specify some of the items and leave some at the default
%           use 0 for the elements to be left at default
%
%		the first element is not used
%     max_its=0;         % max # of total iterations
%     feas_rad=0;        % feasability radius of decision vars
%     per_reduct_its=0;  % # of its done w/ less than 1% relative reduction
%     trace_opt=0;       % if 0, show trace, otherwise don't
%  then,
%		  solv_options=[0,max_its,feas_rad,per_reduct_its,trace_opt];
%
%  gam_init - initial guess at gamma (only used if x_init is specified)
%
%  x_init - initial guess at the vector of decision variables that solve the LMI
%           If this is specified without gam_init, gam_init is calculated
%
% OUTPUTS: --------------  [Dc,Dw1,ubnd1,gam1_pk,xopt1,x_init2,cpu_time]= .
%
%  Dc - the constant scaling matrix associated with struct1 (i.e. TV blocks)
%
%  Dw1 - freq.-dependant scaling associated w/ struct2 (i.e. LTI blocks),
%        (varying matrix)
%
%  ubnd1 - this is the mu upper bound after LMI1 (scaled max singular value),
%          the bound can be lowered at frequencies other than at the peak
%          using LMI2, (varying matrix)
%
%  ubnd1_pk - this is the peak of the mu upper bound for the system being
%            analyzed, if the same freq. pts are used for the second step
%            this shouldn't change (very little anyway)
%
%  xopt1 - this is the vector of decision variables for the optimal solution
%          to this set of LMIs
%
%  x_init2 - this is currently output so as to confuse the user it has very
%            little use at this point
%
%  cpu_time - this is the cpu time in seconds that it took to get the answer
%

% Written by: Lt Paul Blue and Dr. Andy Sparks
%             WL/FIGC-3 Bldg 146
%             2210 Eighth Street Suite 21
%             Wright-Patterson AFB OH 45433-7531
%
%             e-mail: blue,[e-mail]
%             ph: [phone],8682
%
% Copyright (c) 1996, All Rights Reserved


function [Dc,Dw1,ubnd1,gam1_pk,xopt1,x_init2,cpu_time]= ... 
                      lmi1b(M,w,struct1,struct2,solv_options,gam_init,x_init);

%
%  Check for input errors
%
if(nargin<4 | nargin>7);
  disp('usage: [Dc,Dw1,ubnd1,gam1_pk,xopt1,x_init2,cpu_time]= ...');
  disp('            lmi1b(M,w,struct1,struct2,solv_options,gam_init,x_init)');
  return
end

g=M;
omega=w;

if(exist('solv_options')~=1); % i.e. if it doesn't exist
  solv_options=[];
end

if(isempty(solv_options))
  %% then set options here
  %% NOTE: 0=default
  max_its=0;         % max # of total iterations
  feas_rad=0;        % feasability radius of decision vars
  per_reduct_its=0;  % this is the # of its done w/ less than 1% relative reduct
  trace_opt=0;       % if 0, show trace, otherwise don't

  solv_options=[0,max_its,0,per_reduct_its,trace_opt];
end

if(exist('x_init')~=1); % then no IC has been provided
  gam_init=[];
  x_init=[];
end

if(isempty(x_init)==0)    % then an IC has been provided
  if(isempty(gam_init))   % this is because might have a x_init w/o gam_init
    gam_init=25;          % note: after lmi is defined check to make sure this
  end                     %       is big enough
end



%%% check that the inputs are correct form/type/etc


[type,nin,nout,ns] = minfo(g);

if type ~= 'syst';
  error('Not a system matrix')
end

if nin ~= nout;
  error('Not a square system')
end

if max(real(spoles(g))) >= 0;
  error('Not a stable system')
end

if min(size(omega)) ~= 1;
  error('The frequencies must be given as a vector')
end

if size(struct1,2) ~= 2 | min(struct1(:,2)) < 0 | max(struct1(:,2)) > 1;
  error('Structure of the parameters is wrong')
end

if size(struct2,2) ~= 2 | min(struct2(:,2)) < 0 | max(struct2(:,2)) > 1;
  error('Structure of the uncertainty is wrong')
end

if nin ~= (sum(struct1(:,1)) + sum(struct2(:,1)));
  error('The parameter and uncertainty descriptions are inconsistent')
end

%   ***********  end of error checking  **********


ts_cpu=cputime;

%
%  Frequency response of the plant and its conjugate transpose
%

omega = sort(omega);
g_s = frsp(g,omega);
gstar_s = vcjt(g_s);
nfreq = max(size(omega));
nth = sum(struct1(:,1));
nde = sum(struct2(:,1));

%        ********** DEFINE SET OF LMIS **********

setlmis([])
qtheta = lmivar(1,struct1);
lmis1 = getlmis;
ndecth = decnbr(lmis1);

setlmis(lmis1)
for i = 1:nfreq;
  str = ['qdelta',num2str(i),' = lmivar(1,struct2);'];
  eval(str);
end

lmis1 = getlmis;

setlmis(lmis1)
ndec = decnbr(lmis1);
for i = 1:nfreq;
  str = ['q',num2str(i),' = lmivar(3,daug(decinfo(lmis1,qtheta),',...
        'decinfo(lmis1,qdelta',num2str(i),'),decinfo(lmis1,qtheta),',...
        'decinfo(lmis1,qdelta',num2str(i),')));'];
  eval(str);
end



lmiterm([-1 1 1 qtheta],1,1);

for i = 1:nfreq;
  str = ['lmiterm([-(i+1) 1 1 qdelta',num2str(i),'],1,1);'];
  eval(str);
end

for i = 1:nfreq;
  g_i = vunpck(xtract(g_s,omega(i)));
  gs_i = vunpck(xtract(gstar_s,omega(i)));
  bigg = [real(g_i) imag(g_i); -imag(g_i) real(g_i)];
  biggs = [real(gs_i) imag(gs_i); -imag(gs_i) real(gs_i)];
  str = ['lmiterm([(nfreq+1+i) 1 1 q',num2str(i),'],biggs,bigg);'];
  eval(str)
  str = ['lmiterm([-(nfreq+1+i) 1 1 q',num2str(i),'],1,1);'];
  eval(str)
end

lmis1 = getlmis;

% NOTE: there are 2*nfreq+1 LMI's and
%       the last nfreq of them contain a gamma term

% if IC given check gam_init to make sure starting point is feasible
if(isempty(x_init)==0)                % then an IC has been provided
  gamsq_init_min=tinitial(lmis1,x_init,2);
  gam_init_min=sqrt(gamsq_init_min);
  disp(' ');
  disp(['With the given initial values of the decision variables gamma = ',...
        num2str(gam_init_min),';']);
  disp(' ');
  if(gam_init<gam_init_min)
    gam_init=50*gam_init_min;   % NOTE: gam_init_min is feasable but this 
                                %           seems to converge faster
    disp(['gam_init has been adjusted, gam_init = ',num2str(gam_init)]);
    disp(' ');
  end
  gamsq_init=gam_init^2;
end


no_dec_vars=decnbr(lmis1);
disp(['the number of decision variables = ',num2str(no_dec_vars)]);
nlmis=lminbr(lmis1);
disp(['the number of lmis = ',num2str(nlmis)]);
nlmidisc=max(size(lmis1));
disp(['the lmisys vector is ',num2str(nlmidisc),' x 1']);

if(solv_options(5)==0)
  disp(' *** The current value of gamma^2 will be displayed below **');
  disp(' ');
end

[gamsq_opt1,xopt] = gevp(lmis1,nfreq,solv_options,gamsq_init,x_init);
gam1_pk=sqrt(gamsq_opt1);


%
%  The optimal values of dtheta and ddeltai
%
qthetaopt = dec2mat(lmis1,xopt,qtheta);
dtheta = sqrtm(qthetaopt);
x0 = xopt(ndecth+1:ndec);      % NOTE: this is x_init for the lmi2

ddelta0 = [];
gam0 = [];
for i = 1:nfreq;
  str = ['qdelta0 = dec2mat(lmis1,xopt,qdelta',num2str(i),');'];
  eval(str)
%%%%%%%%%%%%% modification for no freq-dependent scalings 
  if isempty(qdelta0)
   ddeltai = [];
  else
   ddeltai = sqrtm(qdelta0);
  end
  ddelta0 = [ddelta0;ddeltai];
  d = daug(dtheta,ddeltai);
  g_i = vunpck(xtract(g_s,omega(i)));
  gam0 = [gam0;max(svd(d*g_i*inv(d)))];
end
% dd0 = vpck(ddelta0/dtheta(1,1),omega);
ddelta=vpck(ddelta0,omega);
ubnd1 = vpck(gam0,omega);

te_cpu=cputime;
cpu_time=te_cpu-ts_cpu;

Dc=dtheta;
Dw1=ddelta;
xopt1=xopt;
x_init2=x0;