lpvlmisol.m

我自己编写的飞行器控制系统设计代码

%calculate the solution to the parameter-dependent output-feedback problem

% vlpv: varying matrix containing "number of the point" N evalution of open
%       loop system matrix
%
% nmeas,nctrl: number of plant measurement and number of the control
%
% vnu: changeing rate bound of the varying parameters 
%      [lbound ubound; ....]
% fparm,gparm: contain basis function data for X and Y.
%
% gradf,gradg; contain partial derivatives of the basis function of X & Y.

function [gam,xmat,ymat,xyopt] = lpvlmisol (vlpv,nmeas,nctrl,vnu,fparm,gparm,gradf,gradg)
tstart = cputime;
% retrieving information for system matrix and basis function.
[mtyp,sysrow,syscol,npts1]=minfo(vlpv);
viv=getiv(vlpv);
[typ,nparm,nbnds,npts6]=minfo(vnu);
npts6=npts1;
vnu=vpck(kron(ones(npts6,1),vnu),viv);

% checking the consistency of the basis function.

[typ,nbasisx,xcol,npts2]= minfo(fparm);
[typ,nbasisy,ycol,npts3]= minfo(gparm);
if (xcol~=1) | (ycol~=1) 
    error('basis data should be colum vector');
end
[typ,nbasisgx,nparmx,npts4]=minfo(gradf);
[typ,nbasisgy,nparmy,npts5]=minfo(gradg);

if (nparmx~=nparmy)
   error('number of parameters in basis function inconsistent');
end

if (nparmx~=nparm)
    error('number of parameter in basis function inconsistent with original 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 rate bound 
igradf = any(vunpck(gradf)); % checking the parameter with 0 entries in gradf
igradg = any(vunpck(gradg));% checking the parameter with 0 entries in gradg
ivun = any(vunpck(vtp(vnu))); % checking parameter without rate bound as 0

parmx = igradf & ivun
parmy = igradg & ivun
nparmx = sum(parmx);
nparmy = sum(parmy);

if nparmx>0
    nvertx=nbnds;
else
    nvertx=2^nparmx;
end

if nparmy>0
    nverty=nbnds;
else
    nverty=2^nparmy
end

% Get matrix containing all combination of vertex
combmatx=corners(nparmx);
combmaty=corners(nparmy);

sys=xtracti(vlpv,1,1);
[systype,no,ni,nx]=minfo(sys);
ne = no-nmeas;
ne1 = ne-nctrl;
nd = ni-nctrl;
nd1 = nd-nmeas;

ndecvar = nx*(nx+1)* (nbasisx+nbasisy)/2 +1 % no. of X (X=X')  + no. of Y (Y=Y') + gamma (1)
nvarxy = nbasisx + nbasisy;


% Setup lmi matrix variable
setlmis([])
for i=1:nbasisx
    X=lmivar(1,[nx,1]);
end

for i=1:nbasisy
    Y=lmivar(1,[nx 1]);
end

gamma=lmivar(1,[1 0]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%LOOP for GRID POINT Adding data to
%%%%%%%%%%%%%%%%%%%%%%%%%%%lmis                                   
for i=1:grid_pts
    disp([' Adding data for grid point ' int2str(i) ' of' int2str(grid_pts)]);
    % getting data for basis function, gradient and rate bound

    fdat=xtracti(fparm,i,1);
    gdat=xtracti(gparm,i,1);
    gfdat=xtracti(fparm,i,1);
    ggdat=xtracti(gparm,i,1);
    nu=xtracti(vnu,i,1);

    % Get state-space system matrices 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)*(i-1);
    indx1 = indx+nverty;
    indx2 = (nvertx+nverty+1)*i;


    %-----------------setting up LMIs for Y, first inequality-----------------------------------------------
    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+nbasisx],gdat(k)*[ahat;c11],ezmaty,'s');
            end
            ly = 0;
            for l = 1:nparm
                if parmy(l) ~= 0
                    ly = ly + 1;
                    parmvl = (nu(l,1)+nu(l,2))/2 + combmaty(j,ly)*(nu(l,1)-nu(l,2))/2;
                    if abs(ggdat(k,l)) > eps & abs(parmvl) > eps
                        lmiterm([indx+j 1 1 k+nbasisx],-parmvl*ggdat(k,l)*ezmaty',ezmaty);
                        %      lmiterm([indx+j 1 1 k+nbasisx],-parmvl*ggdat(k,l),1);
                    end
                end
            end
        end
        lmiterm([indx+j 1 1 nvarxy+1],-1,daug(b2*b2',eye(ne1)));
        lmiterm([indx+j 2 1 0],[b1hat' [d1111 d1112]']);
        lmiterm([indx+j 2 2 nvarxy+1],-1,1);
    end
    %----------------------------------------------------------------------


    %------- setting up LMIs for X, 2nd inequality--------------------------------
    if nd1 > 0
        ezmatx = [eye(nx) zeros(nx,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)*[atld';b11'],ezmatx,'s');
            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);
                    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+1],-1,1);
    end
    %----------------------------------------------------------------

    %-------------setting up LMIs for third inequalities for both X and Y
    delt=1e-6

    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(gdat(k)) > eps
            lmiterm([-indx2 2 2 k], fdat(k),1);
        end
    end
    lmiterm([-indx2 2 2 0],1);
    lmiterm([indx2 1 1 0], delt);
    lmiterm([indx2 2 2 0],delt);

end
%%%%%%%%%%%%%%%%end of grid point loop%%%%%%%%%%%%%%%%%%%%55
lmis=getlmis;

%--------solving the LMIs evaluate the condition----
if ~exist('xyinit')
    xyinit=[];
end
if isempty(xyinit)
    decinit=[];
else
    decinit=xyinit;
end

cobj=zeros(nx*(nx+1)/2*nvarxy,1)
cobj=[cobj;1];

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 to solve the convex optimization
tic
[copt,xyopt]=mincx(lmis,cobj,[1e-2 200 1e7 10 0], decinit ,0);
toc

% calculate solution time and display result.
tthre=cputime;
dtime1=tthre-ttwo;
dtime2=tthre-tstart;
timestr='DONE with optimization, CPU Time=  ';
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

% the output argument
gam=xyopt(ndecvar)
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
xmat=vpck(xmat,[1:nbasisx]')
ymat=vpck(ymat,[1:nbasisy]');