lpvsolbr.m

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

% [gam,xmat,ymat,xyopt] = lpvsolbr(vlpv,nmeas,nctrl,nsf,opt,xyinit,...
%	vnu,fparm,gparm,gradf,gradg,xmat0,ymat0,bparm,gradb)
% Calculates solution to the reduced-order, "blended" parameter-dependent 
% output-and-partial-state feedback problem.
%
% [gam,xmat,ymat,xyopt] = lpvsolbr(vlpv,nmeas,nctrl,nsf,opt,xyinit)
% Calculates solution to the single quadratic Lyapunov problem (no blending).
%
% 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.
% nsf: # of exactly measured states (states & outputs must be ordered last)
% 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 X (or Y) is to be parameter-independent, 
% 	then enter [] for fparm & gradf (gparm & gradg).
% 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 performance level (gamma).
% xmat,ymat: [X11_k X12_k]'s and Y_k's packed in varying matrices.
%	xmat = vpck([X11(1) X12(1);...;X11(N) X12(N)],1:N)
% xyopt: The LMI variables (X11,X12,Y,gam) strung out as a vector.

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

tstart = cputime;
if all(nargin ~= [4 5 6 11 15])
 disp('[gam,xmat,ymat,xyopt] = lpvsolbr(vlpv,nmeas,nctrl,nsf,opt,xyinit,...')
 disp('       vnu,fparm,gparm,gradf,gradg,xmat0,ymat0,bparm,gradb)')
 return
end
disp('  Setting up LMIs...');

% Check for blending option
if nargin > 11 
 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);

if nsf < 1 
 error(' Why use this version for regular output feedback? Use lpvsolb.m')
end

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

if (nargin <= 6)     % Fixed QLF
 PDLF = 0;
 SQLF = 1;
 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] ~= [nx1 nx nx nx])
  error('Error: X and /or Y is the wrong size.');
 end
else
 xmat0 = vpck(zeros(nx1 *nbasisx,nx),1:nbasisx);
 ymat0 = vpck(zeros(nx*nbasisy,nx),1:nbasisy);
end

% Identify the parameters whose rates are bounded (X and/or Y depend on them)
% and are nonzero (vnu_i > 0)
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

% Setup lmi matrix variables
setlmis([])
for i = 1:nbasisx
 X11 = lmivar(1,[nx1 1]); 
end
for i = 1:nbasisx
 X12 = lmivar(2,[nx1 nx2]); 
end
for i = 1:nbasisy
 Y = lmivar(1,[nx 1]); 
end
gamma = lmivar(1,[1 0]);

nvardec = (nx1*(nx1+1)/2+nx1*nx2)*nbasisx + nx*(nx+1)/2*nbasisy + 1;
nvarxy = 2*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.
 gdat = xtracti(gparm,i,1);
 fdat = xtracti(fparm,i,1);
 ggdat = xtracti(gradg,i,1);
 gfdat = xtracti(gradf,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,Bd,Bu,Ce,Cy,Ded] = transfr(sys,nmeas,nctrl,nsf);
 [trow,tcol] = size(Ded);
 if min(eig(eye(tcol)-Ded'*Ded)) <= 0
  disp('I - Ded*Ded < 0');
 end
 B11 = Bd(1:nx1,1:nd1);
 B12 = Bd(1:nx1,nd1+1:nd);
 B21 = Bd(nx1+1:nx,1:nd1);
 B22 = Bd(nx1+1:nx,nd1+1:nd);
 Bd1 = [B11;B21];
 Bd2 = [B12;B22];
 Ce1 = Ce(1:ne1,:);
 Ce2 = Ce(ne1+1:ne,:);
 C11 = Ce(1:ne1,1:nx1);
 C21 = Ce(ne1+1:ne,1:nx1);
 Cy1 = Cy(1:ny1,1:nx1);
 D11 = Ded(1:ne1,1:nd1);
 D12 = Ded(1:ne1,nd1+1:nd);
 D21 = Ded(ne1+1:ne,1:nd1);
 D22 = Ded(ne1+1:ne,nd1+1:nd);
 Ahat = A-Bu*Ce2;
 Bdhat = Bd-Bu*[D21 D22];
 if ny1 == 0 	% (i.e. state feedback only)
  Atld11 = A(1:nx1,1:nx1);
  Atld21 = A(nx1+1:nx,1:nx1);
  Cetld = Ce(:,1:nx1);
 else
  Atld11 = A(1:nx1,1:nx1)-B12*Cy1;
  Atld21 = A(nx1+1:nx,1:nx1)-B22*Cy1;
  Cetld = Ce(:,1:nx1)-[D12;D22]*Cy1;
 end

 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

 if ne1 > 0
  ezmaty = [eye(nx) zeros(nx,ne1)];
 else
  ezmaty = 1;
 end
 for j = 1:nverty
  for k = 1:nbasisy
   if abs(gdat(k)) > eps
    lmiterm([indx+j 1 1 k+2*nbasisx],gdat(k)*[Ahat;Ce1],ezmaty,'s');
%    lmiterm([indx+j 1 1 k+2*nbasisx],gdat(k),Ahat','s');
%    if ne1 > 0
%     lmiterm([indx+j 2 1 k+2*nbasisx],gdat(k)*Ce1,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+2*nbasisx],-parmvl*ggdat(k,l)*ezmaty',ezmaty);
%      lmiterm([indx+j 1 1 k+2*nbasisx],-parmvl*ggdat(k,l),1);
     end
    end
   end
  end
  lmiterm([indx+j 1 1 nvarxy+1],-1,daug(Bu*Bu',eye(ne1)));
  lmiterm([indx+j 2 1 0],[Bdhat' [D11 D12]']);
  lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%  lmiterm([indx+j 1 1 nvarxy+1],-Bu,Bu');
%  if ne1 == 0
%   lmiterm([indx+j 2 1 0],Bdhat');
%   lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%  else
%   lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%   lmiterm([indx+j 3 1 0],Bdhat');
%   lmiterm([indx+j 3 2 0],[D11 D12]');
%   lmiterm([indx+j 3 3 nvarxy+1],-1,1);
%  end
 end

 if nd1 > 0
  ezmatx = [eye(nx1) zeros(nx1,nd1)];
 else
  ezmatx = 1;
 end
 for j = 1:nvertx
  for k = 1:nbasisx
   if abs(fdat(k)) > eps 
    lmiterm([indx1+j 1 1 k],fdat(k)*ezmatx',[Atld11 B11],'s');
    lmiterm([indx1+j 1 1 k+nbasisx],fdat(k)*ezmatx',[Atld21 B21],'s');
%    lmiterm([indx1+j 1 1 k],fdat(k),Atld11,'s');
%    lmiterm([indx1+j 1 1 k+nbasis],fdat(k),Atld21,'s');
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 k],fdat(k)*B11',1);
%     lmiterm([indx1+j 1 2 k+nbasis],fdat(k),B21);
%    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
  if ny1 == 0 	% (i.e. state feedback only)
   lmiterm([indx1+j 1 1 nvarxy+1],-1,daug(zeros(nx1),eye(nd1)));
%   % do nothing for 3x3 block version
  else
   lmiterm([indx1+j 1 1 nvarxy+1],-1,daug(Cy1'*Cy1,eye(nd1)));
%   lmiterm([indx1+j 1 1 nvarxy+1],-Cy1',Cy1);
  end
  lmiterm([indx1+j 2 1 0],[Cetld [D11;D21]]);
  lmiterm([indx1+j 2 2 nvarxy+1],-1,1);
%  if nd1 == 0
%   lmiterm([indx1+j 2 1 0],Cetld);