lpvsolpq.m

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

% [gam,xmat,ymat,xyopt] = ...
%	lpvsolpq(vlpv,dim,vnu,fghparm,gradfg,gamwt,slow,xyinit) 
% calculates the solution to the parameter-dependent OSF LPV control problem
% with dynamic parameter measurement.
%
% INPUTS:
% vlpv:	(VARYING) matrix containing Np evaluations of the OLIC:
%	vlpv = vpck([olic1;...;olicNp],1:Np)
%	Any direct state measurements must be ordered last (in A and in Cy).
% dim = [nmeas nctrl nsf NX NY Ngam]: number of measurements, controls, 
% 	directly measured states, X basis functions, Y basis functions, and 
% 	gamma basis functions. nsf = 0: OF plant. Ng = 0: nonparametric gamma.
% vnu:	Np*Nq-point VARYING (2s x nbnds) matrix grid of velocities for (p,q),
%	where q(s) = (psi s)/(s + psi) p(s) is a filtered estimate of dp/dt.
%	nbnds = 1: -vnu(i)  < d(p,q)(i)/dt < vnu(i)
%	nbnds = 2: vnu(i,1) < d(p,q)(i)/dt < vnu(i,2)
% 	nbnds > 2: vnu(1:s,j) is the j-th corner of the dp/dt polytope.
% 		vnu(s+(1:s),j) is the j-th corner of the dq/dt polytope.
% fghparm = abv(fparm,gparm,hparm): fparm, gparm, and hparm are Np*Nq-point
% 	VARYING basis function data for X(p,q), Y(p,q), and gamma(p,q).
%	Each point of the varying matrix is ((NX+NY+Ngam) x 1).
% gradfg = abv(gradf,gradg): gradf & gradg are Np*Nq-point
% 	VARYING basis gradient data for X(p,q) & Y(p,q).
%	Each point of the varying matrix is ((NX+NY+Ngam) x 2s).
%	To be independent of p(i), use 1 for basis function, 0 for gradient.
% gamwt: grid of scalar weights for parameter-varying gamma,
%	Ngam x 1 CONSTANT or Np*Nq-point VARYING 
% slow = [maxre; gest] (OPTIONAL): maxre = upper bound on |Re(clp poles)|,
%	gest = prior estimate of gam (same size as vunpck(gam))
% xyinit: initial guess for the decision variables (OPTIONAL).
%
% OUTPUTS:
% gam: optimal gamma (scalar CONSTANT or ngam-point VARYING)
% xmat/ymat: The [X11 X12]/Y solutions, as NX/NY-point VARYING matrices.
% xyopt: The vector of optimal decision variables.
%
% c. Lawton H. Lee (1997)

function [gam,xmat,ymat,xyopt] = ...
	lpvsolnew(vlpv,dim,vnu,fghparm,gradfg,gamwt,slow,xyinit);

if nargin < 5 | nargin > 8
 disp('Between 5 and 8 input arguments required. See HELP for details.')
 return;
end

tstart = cputime;
disp('Setting up LMIs...');

% Get some problem dimensions
nmeas = dim(1); nctrl = dim(2); nsf = dim(3); 
NX = dim(4); NY = dim(5); Ng = dim(6);

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

% Get more problem dimensions
[type,no,ni,nx] = minfo(xtracti(vlpv,1,1));
nx1 = nx-nsf; nx2 = nsf;
ny1 = nmeas-nsf; ny2 = nsf;
nd = ni-nctrl; nd1 = nd-ny1;
ne = no-nmeas; ne1 = ne-nctrl;

% Analyze vnu, fghparm, gradfg dimensions
[type,nparm,nbnds,npts1] = minfo(vnu);
if type ~= 'vary'
 vnu = vpck(kron(ones(npts,1),vnu),kron(getiv(fghparm)));
 [type,nparm,nbnds,npts1] = minfo(vnu);
end
if nbnds > 2 
 GRID_PARMV = 1; 
else 
 GRID_PARMV = 0; 
 if nbnds == 1, vnu = sbs(mscl(vnu,-1),vnu); end
end
[dum1,nbasis,ncols,npts2] = minfo(fghparm);
[dum1,ngbasis,nparmg,npts3] = minfo(gradfg);

% If the olic doesn't depend on the filtered rate, then make copies
nq = npts2/npts;
if round(nq) ~= nq
 error('Inconsistent number of q grid points')
end
if nq > 1 
 vlpv = vpck(vunpck(veval('kron',ones(nq,1),vlpv)),getiv(fghparm));
 [type,dum2,dum3,npts] = minfo(vlpv);
 viv = getiv(vlpv);
 disp('Making copies of plant data')
end

% Check for dimension inconsistencies
if (nbasis ~= NX + NY + Ng) | (ngbasis ~= NX + NY)
 error('Number of basis functions inconsistent.');
elseif ncols ~= 1
 error('Basis functions must be vectors.');
elseif nparmg ~= nparm | rem(nparm,2) > 0
 error('Number of parameters inconsistent or odd.');
elseif any(npts ~= [npts2 npts3]) 
 disp(['There are ' int2str(npts) ' points in vlpv'])
 disp(['There are ' int2str(npts1) ' points in vnu'])
 disp(['There are ' int2str(npts2) ' points in fghparm'])
 disp(['There are ' int2str(npts3) ' points in gradfg'])
 error('Number of grid points inconsistent.');
else
 fparm = sel(fghparm,1:NX,':');
 gparm = sel(fghparm,NX+(1:NY),':');
 hparm = sel(fghparm,NX+NY+(1:Ng),':');
 gradf = sel(gradfg,1:NX,':');
 gradg = sel(gradfg,NX+(1:NY),':');
 nparm = nparm / 2;
end

% Identify parameters having finite, nonzero rate bounds
parmx = any(vunpck(gradf)) & any(vunpck(vtp(vnu)));
parmx = parmx(1:nparm) | parmx(nparm+(1:nparm));
parmy = any(vunpck(gradg)) & any(vunpck(vtp(vnu)));
parmy = parmy(1:nparm) | parmy(nparm+(1:nparm));
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

% Check for weight on gamma
if ~exist('gamwt')
 gamwt = 1;
end
gamwt = vunpck(gamwt);
if max(size(gamwt)) <= 1
 PDGAM = 0;
 gamwt = 1;
else
 PDGAM = 1;
 if size(gamwt,1) == 1
  gamwt = gamwt';
 end 
end
ngamwt = length(gamwt);

% Check for parametric gamma
if Ng > 0
 PARGAM = 1;
 ngamvar = Ng;
else
 PARGAM = 0;
 ngamvar = ngamwt;
end

if all(ngamwt ~= [1 npts])
 error('Inconsistent dimensions for gamma weight.');
elseif ~all(gamwt > eps)
 error('All weights on gamma must be positive.');
end
 
% Check for slow option
if ~exist('slow')
 slow = [];
end
if isempty(slow)
 SLOW = 0;
else
 SLOW = 1; maxre = slow(1); gest = slow(2:length(slow));
end

% check for initial variable guess
if ~exist('xyinit')
 xyinit = [];
end

if SLOW, disp('Using pole placement for ''slow'' controller dynamics'), end
if PDGAM, disp('Using parameter-dependent gamma'), end
if ~isempty(xyinit), disp('Using initial guess for variables'), end

% Set up LMI variables
setlmis([]);
for i = 1:NY
 Y = lmivar(1,[nx 1]);
end
for i = 1:NX
 X11 = lmivar(1,[nx1 1]);
end
if nsf > 1
 for i = 1:NX
  X12 = lmivar(2,[nx1 nx2]);
 end
end
for i = 1:ngamvar
 gamma = lmivar(1,[1 0]);
end
nvarxy = (1+sign(nsf)) * NX + NY;
nvardec = (nx1*(nx1+1)/2+sign(nsf)*nx1*nx2)*NX + nx*(nx+1)/2*NY + ngamvar;
cobj = zeros(nvardec-ngamvar,1);
if PARGAM
 cobj = [cobj; vunpck(vtp(hparm))'*gamwt];
else
 cobj = [zeros(nvardec-ngamwt,1); gamwt];
end

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

% Get state-space data for grid point i
 sys = xtracti(vlpv,i,1);
 [A,Bd,Bu,Ce,Cy,Ded] = transfr(sys,nmeas,nctrl,nsf);
 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);
 Dednorm = max(norm([D11 D12]),norm([D11;D21]));
 if Dednorm > 1, disp(['norm(Ded + Deu Dk Dyd) > ' num2str(Dednorm)]); end
 Ahat = A-Bu*Ce2;
 Bdhat = Bd-Bu*[D21 D22];
 if ny1 == 0    % (i.e., no disturbed measurements)
  Atld11 = A(1:nx1,1:nx1);
  Atld21 = A(nx1+1:nx,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

 indx = (nvertx+nverty+1)*(i-1);
 indx1 = indx+nverty;
 indx2 = (nvertx+nverty+1)*i;
 igam = min(ngamvar,i);

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

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

 for j = 1:nverty

  for k = 1:NY
   if abs(gdat(k)) > eps
    lmiterm([indx+j 1 1 k],gdat(k)*[Ahat;Ce1],ezmaty,'s');
%    lmiterm([indx+j 1 1 k],gdat(k),Ahat','s');
%    if ne1 > 0
%     lmiterm([indx+j 2 1 k],gdat(k)*Ce1,1);
%    end
   end
   ly = 0;
   parmv = zeros(2*nparm,1);
   for l = 1:nparm
    ll = nparm + l;
    if parmy(l) ~= 0
     ly = ly + 1;
     if GRID_PARMV
      parmv(l) = nu(l,j);
      parmv(ll) = nu(ll,j);
     else
      parmv(l) = (nu(l,1)+nu(l,2))/2 + combmaty(j,ly)*(nu(l,1)-nu(l,2))/2;
      parmv(ll) = (nu(ll,1)+nu(ll,2))/2 + combmaty(j,ly)*(nu(ll,1)-nu(ll,2))/2;
     end
    end
   end
   if abs(ggdat(k,:)*parmv) > eps 
    lmiterm([indx+j 1 1 k],-(ggdat(k,:)*parmv)*ezmaty',ezmaty);
%    lmiterm([indx+j 1 1 k],-ggdat(k,:)*parmv,1);
   end
  end

  lmiterm([indx+j 2 1 0],[Bdhat' [D11 D12]']);
  if PARGAM & PDGAM
   for k = 1:Ng
    if abs(hdat(k)) > eps
     lmiterm([indx+j 1 1 nvarxy+k],-hdat(k),daug(Bu*Bu',eye(ne1)));
     lmiterm([indx+j 2 2 nvarxy+k],-hdat(k),1);
    end
   end
  else
   lmiterm([indx+j 1 1 nvarxy+igam],-1,daug(Bu*Bu',eye(ne1)));
   lmiterm([indx+j 2 2 nvarxy+igam],-1,1);
  end
%  lmiterm([indx+j 1 1 nvarxy+igam],-Bu,Bu');
%  if ne1 == 0
%   lmiterm([indx+j 2 1 0],Bdhat');
%   lmiterm([indx+j 2 2 nvarxy+igam],-1,1);
%  else
%   lmiterm([indx+j 2 2 nvarxy+igam],-1,1);
%   lmiterm([indx+j 3 1 0],Bdhat');
%   lmiterm([indx+j 3 2 0],[D11 D12]');
%   lmiterm([indx+j 3 3 nvarxy+igam],-1,1);
%  end

 end

 for j = 1:nvertx

  for k = 1:NX
   if abs(fdat(k)) > eps
    lmiterm([indx1+j 1 1 k+NY],fdat(k)*ezmatx',[Atld11 B11],'s');
    if nsf > 1
     lmiterm([indx1+j 1 1 k+NY+NX],fdat(k)*ezmatx',[Atld21 B21],'s');
    end
%    lmiterm([indx1+j 1 1 k+NY],fdat(k),Atld11,'s');
%    if nsf > 1
%     lmiterm([indx1+j 1 1 k+NY+NX],fdat(k),Atld21,'s');
%    end
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 k+NY],fdat(k)*B11',1);
%     if nsf > 1
%      lmiterm([indx1+j 1 2 k+NY+NX],fdat(k),B21);
%     end
%    end
   end
   lx = 0;
   parmv = zeros(2*nparm,1);
   for l = 1:nparm
    ll = nparm + l;
    if parmx(l) ~= 0
     lx = lx + 1;
     if GRID_PARMV
      parmv(l) = nu(l,j);
      parmv(ll) = nu(ll,j);
     else
      parmv(l) = (nu(l,1)+nu(l,2))/2 + combmatx(j,lx)*(nu(l,1)-nu(l,2))/2;
      parmv(ll) = (nu(ll,1)+nu(ll,2))/2 + combmatx(j,lx)*(nu(ll,1)-nu(ll,2))/2;
     end
    end
   end
   if abs(gfdat(k,:)*parmv) > eps
    lmiterm([indx1+j 1 1 k+NY],(gfdat(k,:)*parmv)*ezmatx',ezmatx);
%    lmiterm([indx1+j 1 1 k+NY],gfdat(k,:)*parmv,1);
   end 
  end

  lmiterm([indx1+j 2 1 0],[Cetld [D11;D21]]);
  if PARGAM & PDGAM
   for k = 1:Ng 
    if (abs(hdat(k)) > eps) 
     if ny1 == 0   % (i.e. state feedback only)
      lmiterm([indx1+j 1 1 nvarxy+k],-hdat(k),daug(zeros(nx1),eye(nd1)));
%      % do nothing for 3x3 block version
     else
     lmiterm([indx1+j 1 1 nvarxy+k],-hdat(k),daug(Cy1'*Cy1,eye(nd1)));
%      lmiterm([indx1+j 1 1 nvarxy+igam],-Cy1',Cy1);
     end
     lmiterm([indx1+j 2 2 nvarxy+k],-hdat(k),1);
    end
   end
  else
   if ny1 == 0   % (i.e. state feedback only)
    lmiterm([indx1+j 1 1 nvarxy+igam],-1,daug(zeros(nx1),eye(nd1)));
%    % do nothing for 3x3 block version
   else
   lmiterm([indx1+j 1 1 nvarxy+igam],-1,daug(Cy1'*Cy1,eye(nd1)));
%    lmiterm([indx1+j 1 1 nvarxy+igam],-Cy1',Cy1);
   end
   lmiterm([indx1+j 2 2 nvarxy+igam],-1,1);
  end
%  if nd1 == 0
%   lmiterm([indx1+j 2 1 0],Cetld);
%   lmiterm([indx1+j 2 2 nvarxy+igam],-1,1);
%  else
%   lmiterm([indx1+j 2 2 nvarxy+igam],-1,1);
%   lmiterm([indx1+j 3 1 0],Cetld);
%   lmiterm([indx1+j 3 2 0],[D11;D21]);
%   lmiterm([indx1+j 3 3 nvarxy+igam],-1,1);
%  end

 end

delt = 1e-6;

% Coupling LMI
 for k=1:NY
  if abs(gdat(k)) > eps
   lmiterm([-indx2 1 1 k],gdat(k),1);
  end
 end
 for k=1:NX
  if abs(fdat(k)) > eps
   lmiterm([-indx2 2 2 k+NY],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);
  if PARGAM & PDGAM
   ggest = hdat'*gest;
  else
   ggest = gest(igam);
  end

  for k=1:NY
   if abs(gdat(k)) > eps
    lmiterm([-indx2 1 1 k],gdat(k)*Ahat/(2*maxre),1,'s');
    lmiterm([-indx2 2 1 k],-gdat(k)*C11'*Ce1/(2*maxre*ggest),1);
   end
  end
  for k=1:NX
   if abs(fdat(k)) > eps
    lmiterm([-indx2 2 2 k+NY],1,fdat(k)*Atld11/(2*maxre),'s');
    lmiterm([-indx2 2 1 k+NY],1,-fdat(k)*B11*Bd1'/(2*maxre*ggest));
    if nsf > 0
     lmiterm([-indx2 2 2 k+NY+NX],1,fdat(k)*Atld21/(2*maxre),'s');
     lmiterm([-indx2 2 1 k+NY+NX],1,-fdat(k)*B21*Bd1'/(2*maxre*ggest));
    end
   end
  end
  lmiterm([-indx2 2 1 0],([Bu Bd2]*[C21;Cy1])'/(2*maxre));

  if PARGAM & PDGAM
   for k = 1:Ng
    if abs(hdat(k)) > eps
     lmiterm([-indx2 1 1 nvarxy+k],hdat(k),-Bu*Bu'/maxre);
     if ny1 > 0
      lmiterm([-indx2 2 2 nvarxy+k],hdat(k),-Cy1'*Cy1/maxre);
     end
    end
   end
  else
   lmiterm([-indx2 1 1 nvarxy+igam],1,-Bu*Bu'/maxre);
   if ny1 > 0
    lmiterm([-indx2 2 2 nvarxy+igam],1,-Cy1'*Cy1/maxre);
   end
  end

 end

end % q grid loop
end % grid point loop

lmis = getlmis;

% 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],xyinit,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-ngamvar+(1:ngamvar));
if ngamvar == npts
 gamiv = getiv(fghparm);
else
 gamiv = 1:ngamvar;
end
if PDGAM, gam = vpck(gam,gamiv); end
xmat = [];
ymat = [];
for i = 1:NY
 ymat = [ymat;dec2mat(lmis,xyopt,i)];
end
for i = 1:NX
 if nsf > 0
  xmat = [xmat;dec2mat(lmis,xyopt,i+NY) dec2mat(lmis,xyopt,i+NY+NX)];
 else
  xmat = [xmat;dec2mat(lmis,xyopt,i+NY)];
 end
end
xmat = vpck(xmat,1:NX);
ymat = vpck(ymat,1:NY);