lpvsolnew.m

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

% [gam,xmat,ymat,xyopt] = lpvsolnew(vlpv,dim,vnu,fgparm,gradfg,...
% 	slow,gamwt,xyinit) OR slow,xyinit) OR gamwt,xyinit) OR xyinit)
% calculates the solution to the parameter-dependent OSF LPV control problem.
%
% [gam,xmat,ymat,xyopt] = lpvsolnew(vlpv,dim,...
% 	slow,gamwt,xyinit) OR slow,xyinit) OR gamwt,xyinit) OR xyinit) 
% calculates the solution to the single-quadratic OSF LPV control problem.
%
% INPUTS:
% vlpv:	(VARYING) matrix containing L evaluations of the OLIC:
%	vlpv = vpck([olic1;...;olicL],1:L)
%	Any direct state measurements must be ordered last (in A and in Cy).
% dim = [nmeas nctrl NX NY nsf]: # of measurements, controls, X basis functions,
%	Y basis functions, and (optionally) directly measured states.
% vnu:	(s x nbnds) matrix of constant parameter rate bounds or velocities.
%	nbnds = 1: -vnu(i)   < parm(i) < vnu(i)
%	nbnds = 2: -vnu(i,1) < parm(i) < vnu(i,2)
% 	nbnds > 2: vnu(:,j) is the j-th corner of the velocity polytope.
%	NOTE: vnu can be parameter-dependent (i.e., an L-point VARYING matrix)
% fgparm = abv(fparm,gparm); fparm/gparm contain basis function data for X/Y.
% gradfg = abv(gradf,gradg); gradf/gradg contain basis gradient data for X/Y.
% gamwt: L-point VARYING or L x 1 CONSTANT vector of (positive) weights for 
% 	parameter-dependent gamma. (default = ones(L,1)/L)
% slow = [maxre; gest]: maxre is an upper bound on |Re(closed-loop poles)|,
%	gest = prior estimate of gam (1 x 1 if constant gam, L x 1 if varying) 
% xyinit: initial guess for the decision variables.
%
% OUTPUTS:
% gam: optimal gamma (scalar CONSTANT or L-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 (1996)

function [gam,xmat,ymat,xyopt] = ...
	lpvsolnew(vlpv,dim,arg3,arg4,arg5,arg6,arg7,arg8);

if nargin < 2 | nargin > 8
 disp('Between 2 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); NX = dim(3); NY = dim(4);
if length(dim) > 4
 nsf = dim(5); 
else
 nsf = 0;
end

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

if nargin < 5 
 PDLF = 0;
else
 [type,dum2,dum3,dum4] = minfo(arg5);
 if type == 'cons'
  PDLF = 0;
 else
  PDLF = 1;
 end
end

if PDLF
 vnu = arg3; fgparm = arg4; gradfg = arg5;
else
 vnu = vpck(zeros(npts,1),viv);
 fgparm = vpck(ones(2*npts,1),viv);
 gradfg = vpck(zeros(2*npts,1),viv);
 NX = 1;
 NY = 1;
end

% Analyze vnu dimensions
[type,nparm,nbnds,npts1] = minfo(vnu);
if type ~= 'vary'
 vnu = vpck(kron(ones(npts,1),vnu),viv);
 [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

% Check for dimension inconsistencies
[dum1,nbasis,dum3,npts2] = minfo(fgparm);
[dum1,ngbasis,nparmg,npts3] = minfo(gradfg);
if any([nbasis ngbasis] ~= NX+NY)
 error('Number of basis functions inconsistent.');
elseif nparmg ~= nparm
 error('Number of parameters inconsistent.');
elseif any(npts ~= [npts1 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 fgparm'])
 disp(['There are ' int2str(npts3) ' points in gradfg'])
 error('Number of grid points inconsistent.');
else
 fparm = sel(fgparm,1:NX,':');
 gparm = sel(fgparm,NX+(1:NY),':');
 gradf = sel(gradfg,1:NX,':');
 gradg = sel(gradfg,NX+(1:NY),':');
end

% Identify parameters having finite, nonzero rate bounds
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

if nargin >= 3
 arg3 = vunpck(arg3);
 if nargin >= 4
  arg4 = vunpck(arg4);
  if nargin >= 6
   arg6 = vunpck(arg6);
   if nargin >= 7
    arg7 = vunpck(arg7);
   end
  end
 end
end

% Identify parameter-varying gamma, pole placement and initialization options
if nargin == 2
 SLOW = 0; 
 PDGAM = 0; gamwt = 1;
 xyinit = [];
elseif nargin == 3 
 if any(length(arg3) == [2 1+npts])
  SLOW = 1; maxre = arg3(1); gest = arg3(2:length(arg3)); 
  PDGAM = 0; gamwt = 1;
  xyinit = [];
 elseif length(arg3) == npts
  SLOW = 0; 
  PDGAM = 1; gamwt = arg3;
  xyinit = [];
 else
  SLOW = 0;
  PDGAM = 0; gamwt = 1;
  xyinit = arg3;
 end
elseif nargin == 4
 if any(length(arg3) == [2 1+npts])
  SLOW = 1; maxre = arg3(1); gest = arg3(2:length(arg3));
  if length(vunpck(arg4)) == npts
   PDGAM = 1; gamwt = arg4;
  else
   PDGAM = 0; gamwt = 1;
   xyinit = arg4;
  end
 elseif length(arg3) == npts
  PDGAM = 1; gamwt = arg3;
  if any(length(vunpck(arg4)) == [2 1+npts])
   SLOW = 1; maxre = arg4(1); gest = arg4(2:length(vunpck(arg4)));
  else
   SLOW = 0;
   xyinit = arg4;
  end
 else
  error('Unable to distinguish between gamwt or slow options.');
 end
elseif nargin == 5 
 if PDLF
  SLOW = 0;
  PDGAM = 0; gamwt = 1;
  xyinit = [];
 else
  SLOW = 1; maxre = arg3(1); gest = arg3(2:npts+1);
  PDGAM = 1; gamwt = arg4;
  xyinit = arg5;
 end
elseif nargin == 6
 if any(length(arg6) == [2 npts+1])
  SLOW = 1; maxre = arg6(1); gest = arg6(2:length(vunpck(arg6)));
  PDGAM = 0; gamwt = 1;
  xyinit = [];
 elseif length(arg6) == npts
  SLOW = 0;
  PDGAM = 1; gamwt = arg6;
  xyinit = [];
 else
  SLOW = 0;
  PDGAM = 0; gamwt = 1;
  xyinit = arg6;
 end
elseif nargin == 7
 if length(arg6) == npts
  PDGAM = 1; gamwt = arg6;
  if any(length(arg7) == [2 1+npts])
   SLOW = 1; maxre = arg7(1); gest = arg7(2:length(arg7));
  else
   SLOW = 0;
   xyinit = arg7;
  end
 elseif any(length(arg6) == [2 1+npts])
  SLOW = 1; maxre = arg6(1); gest = arg6(2:length(vunpck(arg6)));
  if length(arg7) == npts
   PDGAM = 1; gamwt = arg7;
  else
   PDGAM = 0; gamwt = 1;
   xyinit = arg7;
  end
 else
  error('Unable to distinguish between gamwt or slow options.');
 end
elseif nargin == 8
 SLOW = 1; maxre = arg6(1); gest = arg6(2:npts+1);
 PDGAM = 1; gamwt = arg7;
 xyinit = arg8;
end

if PDLF, disp('Using PDQ synthesis'), else disp('Using SQLF synthesis'), 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

% Check weight on gamma
if isempty(gamwt), gamwt = ones(ngam,1)/ngam; end
ngam = length(gamwt);
if all(ngam ~= [1 npts])
 error('Inconsistent dimensions for gamma weight.');
elseif ~all(gamwt > eps)
 error('All weights on gamma must be positive.');
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:ngam
 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 + ngam;
cobj = [zeros(nvardec-ngam,1); gamwt];

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

% 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);

% 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

 if PDLF | 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 PDGAM, igam = i; else igam = 1; end

 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(nparm,1);
   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
     parmv(l) = parmvl;
%     if abs(ggdat(k,l)) > eps & abs(parmvl) > eps
%      lmiterm([indx+j 1 1 k],-parmvl*ggdat(k,l)*ezmaty',ezmaty);
%%      lmiterm([indx+j 1 1 k],-parmvl*ggdat(k,l),1);
%     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 1 1 nvarxy+igam],-1,daug(Bu*Bu',eye(ne1)));
  lmiterm([indx+j 2 1 0],[Bdhat' [D11 D12]']);
  lmiterm([indx+j 2 2 nvarxy+igam],-1,1);
%  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(nparm,1);
   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
     parmv(l) = parmvl;
%     if abs(gfdat(k,l)) > eps & abs(parmvl) > eps
%      lmiterm([indx1+j 1 1 k+NY],parmvl*gfdat(k,l)*ezmatx',ezmatx);
%%      lmiterm([indx1+j 1 1 k+NY],parmvl*gfdat(k,l),1);
%     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
  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 1 0],[Cetld [D11;D21]]);
  lmiterm([indx1+j 2 2 nvarxy+igam],-1,1);
%  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-8;

% If PD X and Y
 if PDLF | SLOW
  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);
   for k=1:NY
    lmiterm([-indx2 1 1 k],gdat(k)*Ahat/(2*maxre),1,'s');
    lmiterm([-indx2 2 1 k],-gdat(k)*C11'*Ce1/(2*maxre*gest(igam)),1);
   end
   for k=1:NX
    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*gest(igam)));
    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*gest(igam)));
    end
   end
   lmiterm([-indx2 1 1 nvarxy+igam],1,-Bu*Bu'/maxre);
   if ny1 > 0
    lmiterm([-indx2 2 2 nvarxy+igam],1,-Cy1'*Cy1/maxre);
    lmiterm([-indx2 2 1 0],(Bu*C21+Bd2*Cy1)'/(2*maxre));
   else
    lmiterm([-indx2 2 1 0],(Bu*C21)'/(2*maxre));
   end
  end
 end
end % grid point loop

% If fixed X and Y
if ~PDLF & ~SLOW
 indx2 = 2*npts+1;
 lmiterm([-indx2 1 1 1],1,1);
 lmiterm([-indx2 2 2 2],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

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-ngam+(1:ngam));
if PDGAM, gam = vpck(gam,viv); 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
if PDLF
 xmat = vpck(xmat,1:NX);
 ymat = vpck(ymat,1:NY);
end