lpvsolgr.m

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

% [gam,xmat,ymat,xyopt] = lpvsolgr(vlpv,nmeas,nctrl,nsf,gamwt,opt,xyinit,...
%	vnu,fparm,gparm,gradf,gradg)
% Calculates solution to the reduced-order parameter-dependent 
% output-and-partial-state feedback problem with parameter-varying gamma.
%
% [gam,xmat,ymat,xyopt] = lpvsolgr(vlpv,nmeas,nctrl,nsf,gamwt,opt,xyinit)
% Calculates solution to the single quadratic Lyapunov 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.
% nsf: # of exactly measured states (states & outputs must be ordered last)
% gamwt: N-point VARYING or N x 1 CONSTANT vector of weights for gamma
%       or N x N matrix weight for gamma^2 (objective is a quadratic form).
%       default = ones(N,1)/N, no zeros allowed.
% opt = [maxe gest]: parameters for avoiding "fast" LTI controller dynamics
%       Re(z) > -maxe for all closed-loop (LTI) 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: [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] = ...
 lpvsolgr(vlpv,nmeas,nctrl,nsf,gamwt,opt,xyinit,vnu,fparm,gparm,gradf,gradg);

tstart = cputime;
if all(nargin ~= [4 5 6 7 12])
 disp('[gam,xmat,ymat,xyopt] = lpvsolgr(vlpv,nmeas,nctrl,nsf,...')
 disp('       gamwt,opt,xyinit,vnu,fparm,gparm,gradf,gradg)')
 return
end
disp('  Setting up LMIs...');

% 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 vlpvsol.m')
end

if (nargin <= 7)     % 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);
nx1 = nx-nsf;
nx2 = nsf;
ny1 = nmeas-nsf;
ny2 = nsf;
ne = no-nmeas;
ne1 = ne-nctrl;
nd = ni-nctrl;
nd1 = nd-ny1;

ngam = grid_pts;
nvarxy = 2*nbasisx+nbasisy;
nvardec = (nx1*(nx1+1)/2+nx1*nx2)*nbasisx + nx*(nx+1)/2*nbasisy + ngam;

% Check weights on parameter-varying gamma
if ~exist('gamwt')
 gamwt = [];
end;
if isempty(gamwt)
 gamwt = ones(ngam,1)/ngam;
end
[gtyp,nr,nc,ngam] = minfo(gamwt);
if gtyp == 'vary'
 gamwt = vunpck(gamwt);
else
 ngam = max(nr,nc);
 if nr < nc
  gamwt = gamwt';
 end
end
if any(gamwt < eps)
 gamwt = gamwt+ eps;
end
if max(size(gamwt)) ~= ngam | all(min(size(gamwt)) ~= [1; ngam])
 error('Gamma weight has inconsistent dimensions.')
end

% Check for slow desired controller dynamics
if ~exist('opt')
 opt = [];
end
if isempty(opt)
 SLOW = 0;
else
 SLOW = 1;
 maxe = opt(1);
 gest = opt(2:length(opt));
 if length(gest) ~= ngam
  error('Inconsistent dimensions for gamma estimate')
 end
end

% 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
for i = 1:ngam
 gamma = lmivar(1,[1 0]);
end

% 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);
 Ce1 = Ce(1:ne1,:);
 Ce2 = Ce(ne1+1:ne,:);
 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 | (SQLF & SLOW)
  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+i],-1,daug(Bu*Bu',eye(ne1)));
  lmiterm([indx+j 2 1 0],[Bdhat' [D11 D12]']);
  lmiterm([indx+j 2 2 nvarxy+i],-1,1);
%  lmiterm([indx+j 1 1 nvarxy+i],-Bu,Bu');
%  if ne1 == 0
%   lmiterm([indx+j 2 1 0],Bdhat');
%   lmiterm([indx+j 2 2 nvarxy+i],-1,1);
%  else
%   lmiterm([indx+j 2 2 nvarxy+i],-1,1);
%   lmiterm([indx+j 3 1 0],Bdhat');
%   lmiterm([indx+j 3 2 0],[D11 D12]');
%   lmiterm([indx+j 3 3 nvarxy+i],-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+i],-1,daug(zeros(nx1),eye(nd1)));
%   % do nothing for 3x3 block version
  else
   lmiterm([indx1+j 1 1 nvarxy+i],-1,daug(Cy1'*Cy1,eye(nd1)));
%   lmiterm([indx1+j 1 1 nvarxy+i],-Cy1',Cy1);
  end
  lmiterm([indx1+j 2 1 0],[Cetld [D11;D21]]);
  lmiterm([indx1+j 2 2 nvarxy+i],-1,1);
%  if nd1 == 0
%   lmiterm([indx1+j 2 1 0],Cetld);
%   lmiterm([indx1+j 2 2 nvarxy+i],-1,1);
%  else
%   lmiterm([indx1+j 2 2 nvarxy+i],-1,1);
%   lmiterm([indx1+j 3 1 0],Cetld);
%   lmiterm([indx1+j 3 2 0],[D11;D21]);
%   lmiterm([indx1+j 3 3 nvarxy+i],-1,1);
%  end
 end

delt = 1e-8;

% If PD X and Y
 if PDLF | (SQLF & SLOW)
  for k=1:nbasisy
   if abs(gdat(k)) > eps
    lmiterm([-indx2 1 1 k+2*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],[eye(nx1) zeros(nx1,nx2)]);
  lmiterm([indx2 1 1 0],delt);
  lmiterm([indx2 2 2 0],delt);
  if SLOW
   Bd1 = [B11;B21];
   Bd2 = [B12;B22];
   C11 = Ce(1:ne1,1:nx1);
   C21 = Ce(ne1+1:ne,1:nx1);
   for k=1:nbasisy
    lmiterm([-indx2 1 1 k+2*nbasisx],gdat(k)*Ahat/(2*maxe),1,'s');
    lmiterm([-indx2 2 1 k+2*nbasisx],-gdat(k)*C11'*Ce1/(2*maxe*gest(i)),1);
   end
   for k=1:nbasisx
    lmiterm([-indx2 2 2 k],1,fdat(k)*Atld11/(2*maxe),'s');
    lmiterm([-indx2 2 2 k+nbasisx],1,fdat(k)*Atld21/(2*maxe),'s');
    lmiterm([-indx2 2 1 k],1,-fdat(k)*B11*Bd1'/(2*maxe*gest(i)));
    lmiterm([-indx2 2 1 k+nbasisx],1,-fdat(k)*B21*Bd1'/(2*maxe*gest(i)));
   end
   lmiterm([-indx2 1 1 nvarxy+i],1,-Bu*Bu'/maxe);
   if ny1 > 0
    lmiterm([-indx2 2 2 nvarxy+i],1,-Cy1'*Cy1/maxe);
    lmiterm([-indx2 2 1 0],(Bu*C21+Bd2*Cy1)'/(2*maxe));
   else
    lmiterm([-indx2 2 1 0],(Bu*C21)'/(2*maxe));
   end
  end
 end

end % grid point loop

% If fixed X and Y
if SQLF & (~SLOW)
 indx2 = 2*grid_pts+1;
 lmiterm([-indx2 1 1 3],1,1);
 lmiterm([-indx2 2 2 1],1,1);
 lmiterm([-indx2 2 1 0],[eye(nx1) zeros(nx1,nx2)]);
 lmiterm([indx2 1 1 0],delt);
 lmiterm([indx2 2 2 0],delt);
end

% LMI for the weighted 2-norm gamwt(1)*gam(p1)^2 + ... + gamwt(N)*gam(pN)^2
if size(gamwt,1) == size(gamwt,2)
 indx3 = indx2+1;
 lmiterm([-indx3 1 1 nvarxy+ngam+1],1,1);
 lmiterm([-indx3 2 2 0],inv(gamwt));
 for i = 1:ngam
  lmiterm([-indx3 2 1 nvarxy+i],[zeros(i-1,1);1;zeros(ngam-i,1)],1);
 end
end

lmis=getlmis;  

% Use initial condition if given.
if ~exist('xyinit')
 xyinit = [];
end
if isempty(xyinit)
 decinit = [];
else
 decinit = xyinit;
end
if size(gamwt,1) == size(gamwt,2)
 cobj = [zeros(nx*(nx+1)/2*nvarxy+ngam,1);1];
else
 cobj = [zeros((nx1*(nx1+1)/2+nx1*nx2)*nbasisx+nx*(nx+1)/2*nbasisy,1); 
	gamwt(1:ngam)];
end

% 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
if size(gamwt,1) == size(gamwt,2)
 gam = xyopt(nvardec-ngam-1+(1:ngam));
else
 gam = xyopt(nvardec-ngam+(1:ngam));
end
gam = vpck(gam,viv);
xmat = [];
ymat = [];
for i = 1:nbasisx
 xmat = [xmat;dec2mat(lmis,xyopt,i) dec2mat(lmis,xyopt,i+nbasisx)];
end
for i = 1:nbasisy
 ymat = [ymat;dec2mat(lmis,xyopt,i+2*nbasisx)];
end
if PDLF
 xmat=vpck(xmat,[1:nbasisx]');
 ymat=vpck(ymat,[1:nbasisy]');
end