lpvsol.m

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

% [gam,xmat,ymat,xyopt] = ... 
% 	lpvsol(vlpv,nmeas,nctrl,opt,xyinit,vnu,fparm,gparm,gradf,gradg)
% Calculates solution to the parameter-dependent output-feedback problem.
%
% [gam,xmat,ymat,xyopt] = lpvsol(vlpv,nmeas,nctrl,opt,xyinit)
% Calculates solution to the single quadratic Lyapunov output-feedback problem
%
% H-infinity LMI pole placement (Chilali & Gahinet, 1996) is available.
% We can constrain Re(z) > -maxe for all (LTI) closed-loop poles.
%
% 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. (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).
%
% OUTPUTS: 
% gam: The optimal performance level (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] = ... 
	lpvsol(vlpv,nmeas,nctrl,opt,xyinit,vnu,fparm,gparm,gradf,gradg);

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

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 (nargin <= 5)     % 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;

% 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

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

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

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

% 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,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

 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+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+1],-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+1],-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+1],-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

 if nd1 > 0
  ezmatx = [eye(nx) zeros(nx,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)*[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+1],-1,1);
%  lmiterm([indx1+j 1 1 nvarxy+1],-c2',c2);
%  if nd1 == 0
%   lmiterm([indx1+j 2 1 0],c1tld);
%   lmiterm([indx1+j 2 2 nvarxy+1],-1,1);
%  else
%   lmiterm([indx1+j 2 2 nvarxy+1],-1,1);
%   lmiterm([indx1+j 3 1 0],c1tld);
%   lmiterm([indx1+j 3 2 0],[d1111;d1121]);
%   lmiterm([indx1+j 3 3 nvarxy+1],-1,1);
%  end
 end

delt = 1e-6;

% If PD X and Y
 if PDLF
  for k=1:nbasisy
   if abs(gdat(k)) > eps
    lmiterm([-indx2 1 1 k+nbasisx],gdat(k),1);
   end
  end
  for k=1:nbasisx
   if abs(fdat(k)) > eps
    lmiterm([-indx2 2 2 k],fdat(k),1);
   end
  end
  lmiterm([-indx2 2 1 0],1);
  lmiterm([indx2 1 1 0],delt);
  lmiterm([indx2 2 2 0],delt);
  if SLOW
   for k=1:nbasisy
    if abs(gdat(k)) > eps
     lmiterm([-indx2 1 1 k+nbasisx],gdat(k)/(2*maxe),ahat','s');
     lmiterm([-indx2 2 1 k+nbasisx],-gdat(k)/(2*maxe*gest)*c11'*c11,1);
    end
   end
   for k=1:nbasisx
    if abs(fdat(k)) > eps
     lmiterm([-indx2 2 2 k],fdat(k)/(2*maxe),atld,'s');
     lmiterm([-indx2 2 1 k],-fdat(k)/(2*maxe*gest),b11*b11');
    end
   end
   lmiterm([-indx2 2 2 nvarxy+1],-1/maxe,c2'*c2);
   lmiterm([-indx2 1 1 nvarxy+1],-1/maxe,b2*b2');
   lmiterm([-indx2 2 1 0],(b12*c2+b2*c12)'/(2*maxe));
  end
 end

end % grid point loop

% If fixed X and Y
if SQLF
 indx2 = 2*grid_pts+1;
 lmiterm([-indx2 1 1 2],1,1);
 lmiterm([-indx2 2 2 1],1,1);
 lmiterm([-indx2 2 1 0],1);
 lmiterm([indx2 1 1 0],delt);
 lmiterm([indx2 2 2 0],delt);
 if SLOW
  lmiterm([-indx2 1 1 2],1/(2*maxe),ahat','s');
  lmiterm([-indx2 2 1 2],-1/(2*maxe*gest)*c11'*c11,1);
  lmiterm([-indx2 2 2 1],1/(2*maxe),atld,'s');
  lmiterm([-indx2 2 1 1],-1/(2*maxe*gest),b11*b11');
  lmiterm([-indx2 2 2 nvarxy+1],-1/maxe,c2'*c2);
  lmiterm([-indx2 1 1 nvarxy+1],-1/maxe,b2*b2');
  lmiterm([-indx2 2 1 0],(b12*c2+b2*c12)'/(2*maxe));
 end
end

lmis=getlmis;  

% Use initial condition if given.
if ~exist('xyinit')
 xyinit = [];
end
if isempty(xyinit)
 decinit = [];
else
 decinit = xyinit;
end
cobj = zeros(nx*(nx+1)/2*nvarxy,1);
cobj = [cobj;1];

% Save data to file and display setup time
datesim = date;
timesim = clock;
ttwo = cputime;
dtime = ttwo-tstart;
if dtime < 60
 disp(['Done.  CPU Time = ' num2str(dtime) ' sec']);
elseif dtime < 3600
 disp(['Done.  CPU Time = ' num2str(dtime/60) ' min']);
else
 disp(['Done.  CPU Time = ' num2str(dtime/3600) ' hrs!!']);
end
 
% Call MINCX
tic
[copt,xyopt] = mincx(lmis,cobj,[1e-2 200 1e7 10 0],decinit,0);
toc

% Calculate solution time and display results.
tthre = cputime;
dtime1 = tthre-ttwo;
dtime2 = tthre-tstart;
timestr = 'Done with optimization. CPU Time (LINOBJ) = ';
if dtime1 < 60
 disp([timestr num2str(dtime1) ' sec']);
elseif dtime2 < 3600
 disp([timestr num2str(dtime1/60) ' min']);
else
 disp([timestr num2str(dtime1/3600) ' hrs!!']);
end
if dtime2 < 60
 disp(['                CPU Time (total) = ' num2str(dtime2) ' sec']);
elseif dtime2 < 3600
 disp(['                CPU Time (total) = ' num2str(dtime2/60) ' min']);
else
 disp(['                CPU Time (total) = ' num2str(dtime2/3600) ' hrs!!']);
end
 
if isempty(copt)
 disp(['Sorry. No feasible solution found.']);
 return;
end
 
% Form output arguments
gam = xyopt(nvardec);
xmat = [];
ymat = [];
for i = 1:nbasisx
 xmat = [xmat;dec2mat(lmis,xyopt,i)];
end
for i = 1:nbasisy
 ymat = [ymat;dec2mat(lmis,xyopt,i+nbasisx)];
end
if PDLF
 xmat=vpck(xmat,[1:nbasisx]');
 ymat=vpck(ymat,[1:nbasisy]');
end