lpvsolb.m

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

% [gam,xmat,ymat,xyopt] = lpvsolb(vlpv,nmeas,nctrl,opt,xyinit,... 
% 	vnu,fparm,gparm,gradf,gradg,xmat0,ymat0,bparm,gradb)
% Calculates solution to the "blended" PDLF output-feedback problem.
%
% [gam,xmat,ymat,xyopt] = lpvsolb(vlpv,nmeas,nctrl,opt,xyinit)
% Calculates solution to the SQLF output-feedback problem (not blended).
%
% INPUTS:  
% vlpv: VARYING matrix containing N evaluations of the open-loop IC's:
% 	  vlpv = vpck([olic(parm1);...;olic(parmN)],1:N)
% nmeas,nctrl: number of plant measurements and number of controls.
% opt = [maxe gest]: parameters for avoiding "fast" LTI controller dynamics.
%       Re(z) > -maxe for all closed-loop poles, gest is a prior estimate
%       for gam. See Chilali/Gahinet (1996). (OPTIONAL, enter [] for none)
% xyinit: initial guess for the LMI variable. (OPTIONAL, enter [] for none)
% vnu:  (s x 1) [(s x 2)] matrix of [lower and upper] rate bounds on all s
% 	parameters, with garbage entries for parameters without rate bounds:
% 	  -nu(i) <= parm(i) <= nu(i)  OR  nu(i,1) <= parm(i) <= nu(i,2) 
% 	NOTE: vnu can be parameter-varying, i.e. a VARYING matrix like vlpv.
% 	NOTE: vnu with more than two columns is considered a grid of 
% 	parameter velocities, e.g. vertices of the velocity polytope. 
% fparm,gparm: contains basis function data for X & Y.
% gradf,gradg: contains partial derivatives of basis functions for X & Y.
% 	NOTE: if either X or Y (but not both) is to be parameter-dependent, 
% 	then the basis function data of the fixed one should be entered as [].
% xmat0,ymat0: previous (X,Y) solutions to be blended with this one.
% bparm: VARYING interpolation function values at selected grid points. The 
%	function is 1 outside the initial region, 0 outside the current one.
% gradb: VARYING interpolation function gradients at selected grid points.
%
% OUTPUTS: 
% gam: The optimal gamma.
% xmat,ymat: X_k's and Y_k's packed in varying matrices.
% xyopt: The LMI variables (X,Y,gam) strung out as a vector.

function [gam,xmat,ymat,xyopt] = lpvsolb(vlpv,nmeas,nctrl,opt,xyinit,... 
	vnu,fparm,gparm,gradf,gradg,xmat0,ymat0,bparm,gradb)

tstart = cputime;
if all(nargin ~= [3 4 5 10 14]) 
 disp('usage: [gam,xmat,ymat,xyopt] = lpvsolb(vlpv,nmeas,nctrl,opt,xyinit,...')
 disp('       vnu,fparm,gparm,gradf,gradg,xmat0,ymat0,bparm,gradb)')
 return
end
disp('  Setting up LMIs...');

% Check for blending option
if nargin  == 14
 BLEND = 1;
else
 BLEND = 0;
end

% Check for pole placement option
if ~exist('opt')
 opt = [];
end
if isempty(opt)
 SLOW = 0;
else
 SLOW = 1;
 maxe = opt(1);
 gest = opt(2);
end

% If vlpv is a system matrix, pack it as a varying matrix.
[typ,dum2,dum3,npts1] = minfo(vlpv);
if typ == 'syst'
 vlpv = vpck(vlpv,1);
 [typ,dum2,dum3,npts1] = minfo(vlpv);
end
viv = getiv(vlpv);

sys = xtracti(vlpv,1,1);
[systype,no,ni,nx] = minfo(sys);
ne = no-nmeas;
ne1 = ne-nctrl;
nd = ni-nctrl;
nd1 = nd-nmeas;

if ne1 > 0
 ezmaty = [eye(nx) zeros(nx,ne1)];
else
 ezmaty = 1;
end
if nd1 > 0
 ezmatx = [eye(nx) zeros(nx,nd1)];
else
 ezmatx = 1;
end

if (nargin <= 5)     % Fixed QLF
 SQLF = 1;
 PDLF = 0;
 viv = getiv(vlpv);
 vnu = vpck(zeros(npts1,1),viv);
 fparm = [];
 gparm = [];
 gradf = [];
 gradg = [];
 nparmx = 0;
 nparmy = 0;
 nvertx = 1;
 nverty = 1;
else
 PDLF = 1;
 SQLF = 0;
end

% If vnu is a constant vector, pack it as a varying matrix.
[typ,nparm,nbnds,npts6] = minfo(vnu);
if typ ~= 'vary'
 [nparm,nbnds] = size(vnu);
 npts6 = npts1;
 vnu = vpck(kron(ones(npts6,1),vnu),viv);
end
if nbnds > 2
 GRID_PARMV = 1;
else
 GRID_PARMV = 0;
end

if (isempty(fparm))
 fparm = vpck(ones(npts1,1),viv);
 gradf = vpck(zeros(npts1,nparm),viv);
end
if (isempty(gparm))
 gparm = vpck(ones(npts1,1),viv);
 gradg = vpck(zeros(npts1,nparm),viv);
end

% Check that the number of grid points is consistent.
[dum1,nbasisx,ckvx,npts2] = minfo(fparm);
[dum1,nbasisy,ckvy,npts3] = minfo(gparm);
if (ckvx ~= 1) | (ckvy ~= 1)
 error('Basis data should be column vectors.');
end
[dum1,nbasisgx,nparmgx,npts4] = minfo(gradf);
[dum1,nbasisgy,nparmgy,npts5] = minfo(gradg);
if (nparmgx ~= nparmgy) 
 error('Number of parameters in basis gradient data inconsistent.');
end
if (nparmgx ~= nparm) 
 error('Number of parameters in nu inconsistent with gradient data.');
end
if (nbasisgx ~= nbasisx) | (nbasisy ~= nbasisgy) 
 error('Number of basis not consistent.');
end
if (npts1 ~= npts2) | (npts2 ~= npts3) | (npts3 ~= npts4) | ...
	(npts4 ~= npts5) | (npts5 ~= npts6) | (npts6 ~= npts1)
 disp(['There are ' int2str(npts1) ' points in vlpv']);
 disp(['There are ' int2str(npts2) ' points in fparm']);
 disp(['There are ' int2str(npts3) ' points in gparm']);
 disp(['There are ' int2str(npts4) ' points in gradf']);
 disp(['There are ' int2str(npts5) ' points in gradg']);
 disp(['There are ' int2str(npts6) ' points in vnu']);
 error('Number of grid points inconsistent');
end  
grid_pts = npts1;

% Check dimensions of blending data
if BLEND
 [dum1,nblends,ckvb,nbpts] = minfo(bparm);
 [dum1,ngblends,nparmgb,nbpts1] = minfo(gradb);
 if nparmgb ~= nparm
  error('Error: Number of parameters in blending function inconsistent.');
 end
 if any([ngblends nblends ckvb] > 1)
  error('Error: Only one scalar blending function allowed.');
 end
 if nbpts ~= nbpts1
  error('Error: Inconsistent number of blended grid points.');
 end
else
 bparm = [];
 gradb = [];
 nbpts = 0;
end
biv = getiv(bparm);

% Check dimensions of xmat0,ymat0
if BLEND
 [typ,xrow,xcol,numx] = minfo(xmat0);
 if typ == 'cons'
  xmat0 = vpck(xmat0,1);
  [typ,xrow,xcol,numx] = minfo(xmat0);
 end
 [typ,yrow,ycol,numy] = minfo(ymat0);
 if typ == 'cons'
  ymat0 = vpck(ymat0,1);
  [typ,yrow,ycol,numy] = minfo(ymat0);
 end
 if (numx ~= nbasisx) | (numy ~= nbasisy) 
  error('Error: Number of basis functions not consistent with initial X,Y');
 end
 if any([xrow xcol yrow ycol] ~= nx)
  error('Error: X and /or Y is the wrong size.');
 end
else
 xmat0 = vpck(zeros(nx*nbasisx,nx),1:nbasisx);
 ymat0 = vpck(zeros(nx*nbasisy,nx),1:nbasisy);
end

% Identify the parameters on which X and Y should vary
parmx = any(vunpck(gradf)) & any(vunpck(vtp(vnu)));
parmy = any(vunpck(gradg)) & any(vunpck(vtp(vnu)));
nparmx = sum(parmx);
nparmy = sum(parmy);
if GRID_PARMV & nparmx > 0
 nvertx = nbnds;
else
 nvertx = 2^nparmx;
end
if GRID_PARMV & nparmy > 0
 nverty = nbnds;
else
 nverty = 2^nparmy;
end
 
% Get matrix containing all combinations of +/- for nparm-dim vector.
combmatx = corners(nparmx);	% combmatx = 1 if nparmx = 0
combmaty = corners(nparmy);	% combmaty = 1 if nparmy = 0

ngam = 1;
nvardec = nx*(nx+1)*(nbasisx+nbasisy)/2+ngam;

% Setup lmi matrix variables
setlmis([])
for i = 1:nbasisx
 X = lmivar(1,[nx 1]); 		% X
end
for i = 1:nbasisy
 Y = lmivar(1,[nx 1]); 		% Y
end
for i = 1:ngam
 gamma = lmivar(1,[1 0]);	% gamma (gam_d & gam_e)
end
nvarxy = nbasisx+nbasisy;

% Add data to variable "lmis".
for i = 1:grid_pts
 disp([' Adding data for grid point ' int2str(i) ' of ' int2str(grid_pts)]);

% Get values of basis functions, gradient, and rate bound for parm_i.
 fdat = xtracti(fparm,i,1);
 gdat = xtracti(gparm,i,1);
 gfdat = xtracti(gradf,i,1);
 ggdat = xtracti(gradg,i,1);
 nu = xtracti(vnu,i,1);

%  Check for a two-sided rate bound, or just a bound on absolute value
 if nbnds == 1
  nu = [-nu nu];
 end

% Get state-space data for grid point i
 sys = xtracti(vlpv,i,1);
 [a,b1,b2,c1,c2,d11] = transf(sys,nmeas,nctrl);
 [trow,tcol] = size(d11);
 if min(eig(eye(tcol)-d11'*d11)) <= 0
  disp('I - D11*D11 < 0');
 end
 b11 = b1(:,1:nd1);
 b12 = b1(:,nd1+1:nd);
 c11 = c1(1:ne1,:);
 c12 = c1(ne1+1:ne,:);
 d1111 = d11(1:ne1,1:nd1);
 d1112 = d11(1:ne1,nd1+1:nd);
 d1121 = d11(ne1+1:ne,1:nd1);
 d1122 = d11(ne1+1:ne,nd1+1:nd);
 ahat = a-b2*c12;
 atld = a-b12*c2;
 b1hat = b1-b2*[d1121 d1122];
 c1tld = c1-[d1112;d1122]*c2;
   
 if PDLF
  indx = (nvertx+nverty+1)*(i-1);
  indx1 = indx+nverty;
  indx2 = (nvertx+nverty+1)*i;
 else 
  indx = 2*(i-1);
  indx1 = indx+1;
 end

 for j = 1:nverty
  for k = 1:nbasisy
   if abs(gdat(k)) > eps
    lmiterm([indx+j 1 1 k+nbasisx],gdat(k)*[ahat;c11],ezmaty,'s');
%    lmiterm([indx+j 1 1 k+nbasisx],gdat(k),ahat','s');
%    if ne1 > 0
%     lmiterm([indx+j 2 1 k+nbasisx],gdat(k)*c11,1);
%    end
   end
   ly = 0;
   for l = 1:nparm
    if parmy(l) ~= 0
     ly = ly + 1;
     if GRID_PARMV
      parmvl = nu(l,j);
     else
      parmvl = (nu(l,1)+nu(l,2))/2 + combmaty(j,ly)*(nu(l,1)-nu(l,2))/2;
     end 
     if abs(ggdat(k,l)) > eps & abs(parmvl) > eps
      lmiterm([indx+j 1 1 k+nbasisx],-parmvl*ggdat(k,l)*ezmaty',ezmaty);
%      lmiterm([indx+j 1 1 k+nbasisx],-parmvl*ggdat(k,l),1);
     end
    end
   end
  end
  lmiterm([indx+j 1 1 nvarxy+ngam],-1,daug(b2*b2',eye(ne1)));
  lmiterm([indx+j 2 1 0],[b1hat' [d1111 d1112]']);
  lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%  lmiterm([indx+j 1 1 nvarxy+ngam],-b2,b2');
%  if ne1 == 0
%   lmiterm([indx+j 2 1 0],b1hat');
%   lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%  else
%   lmiterm([indx+j 2 2 nvarxy+ngam],-1,1);
%   lmiterm([indx+j 3 1 0],b1hat');
%   lmiterm([indx+j 3 2 0],[d1111 d1112]');
%   lmiterm([indx+j 3 3 nvarxy+1],-1,1);
%  end
 end

 for j = 1:nvertx
  for k = 1:nbasisx
   if abs(fdat(k)) > eps
    lmiterm([indx1+j 1 1 k],fdat(k)*[atld';b11'],ezmatx,'s');
%    lmiterm([indx1+j 1 1 k],fdat(k),atld,'s');
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 k],fdat(k)*b11',1);
%    end
   end
   lx = 0;
   for l = 1:nparm
    if parmx(l) ~= 0
     lx = lx + 1;
     if GRID_PARMV
      parmvl = nu(l,j);
     else
      parmvl = (nu(l,1)+nu(l,2))/2 + combmatx(j,lx)*(nu(l,1)-nu(l,2))/2;
     end
     if abs(gfdat(k,l)) > eps & abs(parmvl) > eps
      lmiterm([indx1+j 1 1 k],parmvl*gfdat(k,l)*ezmatx',ezmatx);
%      lmiterm([indx1+j 1 1 k],parmvl*gfdat(k,l),1);
     end
    end  
   end
  end
  lmiterm([indx1+j 1 1 nvarxy+1],-1,daug(c2'*c2,eye(nd1)));
  lmiterm([indx1+j 2 1 0],[c1tld [d1111;d1121]]);
  lmiterm([indx1+j 2 2 nvarxy+ngam],-1,1);
%  lmiterm([indx1+j 1 1 nvarxy+1],-c2',c2);
%  if nd1 == 0