lpvsolb.m

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

%   lmiterm([indx1+j 2 1 0],c1tld);
%   lmiterm([indx1+j 2 2 nvarxy+ngam],-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+ngam],-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

% set up extra LMIs at each blending point
fparm = xtract(fparm,biv);
gparm = xtract(gparm,biv);
gradf = xtract(gradf,biv);
gradg = xtract(gradg,biv);
vnu = xtract(vnu,biv);
bparm = xtract(bparm,biv);
gradb = xtract(gradb,biv);
vlpv = xtract(vlpv,biv);

for i = 1:nbpts
 disp([' Adding data for blending point ' int2str(i) ' of ' int2str(nbpts)]);

% Evaluate basis & blending functions, gradients, and rate bound.
 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);
 bdat = xtracti(bparm,i,1);
 gbdat = xtracti(gradb,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;
   
 indx = (nvertx+nverty+1) * (grid_pts + i-1);
 indx1 = indx+nverty;
 indx2 = (nvertx+nverty+1) * (grid_pts + i);

 for j = 1:nverty
  for k = 1:nbasisy
   Yk0 = xtracti(ymat0,k,1);
   if (bdat > eps) & abs(gdat(k)) > eps
    lmiterm([indx+j 1 1 k+nbasisx],bdat*gdat(k)*[ahat;c11],ezmaty,'s');
%    lmiterm([indx+j 1 1 k+nbasisx],bdat*gdat(k),ahat','s');
%    if ne1 > 0
%     lmiterm([indx+j 2 1 k+nbasisx],bdat*gdat(k)*c11,1);
%    end
   end
   if (1-bdat > eps) & abs(gdat(k)) > eps
    mat = [ahat;c11]*Yk0*ezmaty;
    lmiterm([indx+j 1 1 0],(1-bdat)*gdat(k)*(mat+mat'));
%    lmiterm([indx+j 1 1 0],(1-bdat)*gdat(k)*(ahat*Yk0 + Yk0*ahat'));
%    if ne1 > 0
%     lmiterm([indx+j 2 1 0],(1-bdat)*gdat(k)*c11*Yk0);
%    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(parmvl) > eps 
      if max([min(abs([bdat ggdat(k,l)])) min(abs([gbdat(1,l) gdat(k)]))]) > eps
       fac = bdat*ggdat(k,l)+gbdat(1,l)*gdat(k);
       lmiterm([indx+j 1 1 k+nbasisx],-parmvl*fac*ezmaty',ezmaty);
%       lmiterm([indx+j 1 1 k+nbasisx],-parmvl*fac,1);
      end
      if max([min(abs([1-bdat ggdat(k,l)])) min(abs([gbdat(1,l) gdat(k)]))]) > eps
       fac = (1-bdat)*ggdat(k,l)-gbdat(1,l)*gdat(k);
       lmiterm([indx+j 1 1 0],-parmvl*fac*ezmaty'*Yk0*ezmaty);
%       lmiterm([indx+j 1 1 0],-parmvl*fac*Yk0);
      end
     end
    end
   end
  end
  lmiterm([indx+j 1 1 nvarxy+ngam],-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+ngam],-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+ngam],-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

 for j = 1:nvertx
  for k = 1:nbasisx
   Xk0 = xtracti(xmat0,k,1);
   if (abs(fdat(k)) > eps) & (bdat > eps)
    lmiterm([indx1+j 1 1 k],bdat*fdat(k)*[atld';b11'],ezmatx,'s');
%    lmiterm([indx1+j 1 1 k],bdat*fdat(k),atld,'s');
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 k],bdat*fdat(k)*b11',1);
%    end
   end
   if (abs(fdat(k)) > eps) & (1-bdat > eps) 
    mat = [atld';b11']*Xk0*ezmatx;
    lmiterm([indx1+j 1 1 0],(1-bdat)*fdat(k)*(mat+mat'));
%    lmiterm([indx1+j 1 1 0],(1-bdat)*fdat(k)*(atld'*Xk0 + Xk0*atld));
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 0],(1-bdat)*fdat(k)*b11'*Xk0);
%    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(parmvl) > eps 
      if max([min(abs([bdat gfdat(k,l)])) min(abs([gbdat(1,l) fdat(k)]))]) > eps
       fac = bdat*gfdat(k,l)+gbdat(1,l)*fdat(k);
       lmiterm([indx1+j 1 1 k],parmvl*fac*ezmatx',ezmatx);
%       lmiterm([indx1+j 1 1 k],parmvl*fac,1);
      end
      if max([min(abs([1-bdat gfdat(k,l)])) min(abs([gbdat(1,l) fdat(k)]))]) > eps 
       fac = (1-bdat)*gfdat(k,l)-gbdat(1,l)*fdat(k);
       lmiterm([indx1+j 1 1 0],parmvl*fac*ezmatx'*Xk0*ezmatx);
%       lmiterm([indx1+j 1 1 0],parmvl*fac*Xk0);
      end 
     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+ngam],-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+ngam],-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+ngam],-1,1);
%  end
 end

 for k=1:nbasisy
  Yk0 = xtracti(ymat0,k,1);
  if (abs(gdat(k)) > eps) & (bdat > eps)
   lmiterm([-indx2 1 1 k+nbasisx],bdat*gdat(k),1);
  end
  if (abs(gdat(k)) > eps) & (1 - bdat > eps) 
   lmiterm([-indx2 1 1 0],(1-bdat)*gdat(k)*Yk0);
  end
 end
 for k=1:nbasisx
  Xk0 = xtracti(xmat0,k,1);
  if (abs(fdat(k)) > eps) & (bdat > eps)
   lmiterm([-indx2 2 2 k],bdat*fdat(k),1);
  end
  if (abs(fdat(k)) > eps) & (1 - bdat > eps)
   lmiterm([-indx2 2 2 0],(1-bdat)*fdat(k)*Xk0);
  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
   Yk0 = xtracti(ymat0,k,1);
   if (abs(gdat(k)) > eps) & (bdat > eps)
    lmiterm([-indx2 1 1 k+nbasisx],bdat*gdat(k)/(2*maxe),ahat','s');
    lmiterm([-indx2 2 1 k+nbasisx],-bdat*gdat(k)/(2*maxe*gest)*c11'*c11,1);
   end
   if (abs(gdat(k)) > eps) & (1 - bdat > eps) 
    lmiterm([-indx2 1 1 0],(1-bdat)*gdat(k)/(2*maxe)*(ahat*Yk0+Yk0*ahat'));
    lmiterm([-indx2 2 1 0],-(1-bdat)*gdat(k)/(2*maxe*gest)*c11'*c11*Yk0);
   end
  end
  for k=1:nbasisx
   Xk0 = xtracti(xmat0,k,1);
   if (abs(fdat(k)) > eps) & (bdat > eps)
    lmiterm([-indx2 2 2 k],bdat*fdat(k)/(2*maxe),atld,'s');
    lmiterm([-indx2 2 1 k],-bdat*fdat(k)/(2*maxe*gest),b11*b11');
   end
   if (abs(fdat(k)) > eps) & (1 - bdat > eps)
    lmiterm([-indx2 2 2 0],(1-bdat)*fdat(k)/(2*maxe)*(atld'*Xk0+Xk0*atld));
    lmiterm([-indx2 2 1 0],-(1-bdat)*fdat(k)/(2*maxe*gest)*Xk0*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 % blended grid point loop

lmis=getlmis;  

% Use initial condition if given.
if ~exist('xyinit') 
 xyinit = [];
end
if ~isempty(xyinit)
 decinit = xyinit;
else
 decinit = [];
end
cobj = zeros(nvardec,1);
cobj(nvardec-ngam+(1:ngam)) = ones(ngam,1)/ngam;

% 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
 
gam = xyopt(nvardec-(ngam-1):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