lpvsolbr.m

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

%   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],Cetld);
%   lmiterm([indx1+j 3 2 0],[D11;D21]);
%   lmiterm([indx1+j 3 3 nvarxy+1],-1,1);
%  end
 end

delt = 1e-8;

% If PD X and Y
 if PDLF
  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
   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),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));
    lmiterm([-indx2 2 1 k+nbasisx],1,-fdat(k)*B21*Bd1'/(2*maxe*gest));
   end
   lmiterm([-indx2 1 1 nvarxy+1],1,-Bu*Bu'/maxe);
   if ny1 > 0
    lmiterm([-indx2 2 2 nvarxy+1],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
 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);
 if SLOW
  lmiterm([-indx2 1 1 3],Ahat/(2*maxe),1,'s');
  lmiterm([-indx2 2 1 3],-C11'*Ce1/(2*maxe*gest),1);
  lmiterm([-indx2 2 2 1],1,Atld11/(2*maxe),'s');
  lmiterm([-indx2 2 2 2],1,Atld21/(2*maxe),'s');
  lmiterm([-indx2 2 1 1],1,-B11*Bd1'/(2*maxe*gest));
  lmiterm([-indx2 2 1 2],1,-B21*Bd1'/gest/(2*maxe));
  lmiterm([-indx2 1 1 nvarxy+1],1,-Bu*Bu'/maxe);
  if ny1 > 0
   lmiterm([-indx2 2 2 nvarxy+1],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

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

 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+2*nbasisx],bdat*gdat(k)*[Ahat;Ce1],ezmaty,'s');
%    lmiterm([indx+j 1 1 k+2*nbasisx],bdat*gdat(k),Ahat','s');
%    if ne1 > 0
%     lmiterm([indx+j 2 1 k+2*nbasisx],bdat*gdat(k)*Ce1,1);
%    end
   end
   if (1-bdat > eps) & abs(gdat(k)) > eps
    mat = [Ahat;Ce1]*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)*Ce1*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+2*nbasisx],-parmvl*fac*ezmaty',ezmaty);
%       lmiterm([indx+j 1 1 k+2*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+1],-1,daug(Bu*Bu',eye(ne1)));
  lmiterm([indx+j 2 1 0],[Bdhat' [D11 D12]']);
  lmiterm([indx+j 2 2 nvarxy+1],-1,1);
%  lmiterm([indx+j 1 1 nvarxy+1],-Bu,Bu');
%  if ne1 == 0
%   lmiterm([indx+j 2 1 0],Bdhat');
%   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],Bdhat');
%   lmiterm([indx+j 3 2 0],[D11 D12]');
%   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 bdat > eps & abs(fdat(k)) > eps 
    lmiterm([indx1+j 1 1 k],bdat*fdat(k)*ezmatx',[Atld11 B11],'s');
    lmiterm([indx1+j 1 1 k+nbasisx],bdat*fdat(k)*ezmatx',[Atld21 B21],'s');
%    lmiterm([indx1+j 1 1 k],bdat*fdat(k),Atld11,'s');
%    lmiterm([indx1+j 1 1 k+nbasis],bdat*fdat(k),Atld21,'s');
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 k],bdat*fdat(k)*B11',1);
%     lmiterm([indx1+j 1 2 k+nbasis],bdat*fdat(k),B21);
%    end
   end
   if 1 - bdat > eps & abs(fdat(k)) > eps 
    mat = ezmatx'*Xk0*[[Atld11;Atld21] Bd1];
    lmiterm([indx1+j 1 1 0],(1-bdat)*fdat(k)*(mat+mat'));
%    lmiterm([indx1+j 1 1 0],(1-bdat)*fdat(k)*([Atld11;Atld21]'*Xk0'+Xk0*[Atld11;Atld21]));
%    if nd1 > 0
%     lmiterm([indx1+j 2 1 0],(1-bdat)*fdat(k)*(Bd1'*Xk0'+Xk0*Bd1));
%    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)]))]) > e
ps
       fac = (1-bdat)*gfdat(k,l)-gbdat(1,l)*fdat(k);
       lmiterm([indx1+j 1 1 0],parmvl*fac*ezmatx'*Xk0(:,1:nx1)*ezmatx);
%       lmiterm([indx1+j 1 1 0],parmvl*fac*Xk0);
      end
     end
    end  
   end
  end
  if ny1 == 0 	% (i.e. state feedback only)
   lmiterm([indx1+j 1 1 nvarxy+1],-1,daug(zeros(nx1),eye(nd1)));
%   % do nothing for 3x3 block version
  else
   lmiterm([indx1+j 1 1 nvarxy+1],-1,daug(Cy1'*Cy1,eye(nd1)));
%   lmiterm([indx1+j 1 1 nvarxy+1],-Cy1',Cy1);
  end
  lmiterm([indx1+j 2 1 0],[Cetld [D11;D21]]);
  lmiterm([indx1+j 2 2 nvarxy+1],-1,1);
%  if nd1 == 0
%   lmiterm([indx1+j 2 1 0],Cetld);
%   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],Cetld);
%   lmiterm([indx1+j 3 2 0],[D11;D21]);
%   lmiterm([indx1+j 3 3 nvarxy+1],-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+2*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(:,1:nx1));
  end
 end
 lmiterm([-indx2 2 1 0],[eye(nx1) zeros(nx1,nx2)]);
 lmiterm([indx2 1 1 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+2*nbasisx],bdat*gdat(k)/(2*maxe),ahat','s');
    lmiterm([-indx2 2 1 k+2*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],1,bdat(k)*fdat(k)*Atld11/(2*maxe),'s');
    lmiterm([-indx2 2 2 k+nbasisx],1,bdat(k)*fdat(k)*Atld21/(2*maxe),'s');
    lmiterm([-indx2 2 1 k],-bdat(k)*fdat(k),B11*Bd1'/(2*maxe*gest));
    lmiterm([-indx2 2 1 k+nbasisx],-bdat(k)*fdat(k),B21*Bd1'/(2*maxe*gest));
   end
   if (abs(fdat(k)) > eps) & (1 - bdat > eps)
    atld = [Atld11;Atld21];
    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*Bd1*Bd1');
   end
  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 % blended grid point loop

lmis=getlmis;  

% Use initial condition if given.
if ~exist('xyinit')
 xyinit = [];
end
if isempty(xyinit)
 decinit = [];
else
 decinit = xyinit;
end
cobj = zeros(nvardec-1,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) 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