mysd.m

一个目标跟踪系统的MATLAB 源程序包

% Jun.9,1999, a subroutine of SSD.m
% use Accelerated Subgradient Update method

function [gam]=mySD(c)

n=size(c);
S=ndims(c);

if S==2
   gam=[1:n(1)]';
   [J,omiga,assign]=auction(-c);
   assi = find_assi_index(assign, n(1));
   gam=[gam assi];
   return;    
end   

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 1: Initialize
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u=zeros(S,max(n));
f_dual=-realmax;
f_primal=realmax;
rgap=realmax;
iter=0;
mingap=.02;  %  typically 0.01 to 0.05
maxiter=500;

a=.2;   % typically, .05<= a <=.3
b=1.2;  % typically, 1.1<= b <=1.6
beta=1;
alpha=2;
g=ones(S,max(n));
H=zeros(max(n),max(n),S);

while rgap>mingap & iter<maxiter
   
   gam=[1:n(1)]';

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % STEP 2: Compute reduced costs for r=S-1, S-2,...,2.
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % 2A: c-u
   for r=2:S-1 
         rowlength=prod(n(1:r));
         collength=prod(n(r+1:S));
         c2=reshape(c,rowlength,collength);
         
         col2=collength/n(r+1);
         row2=n(r+1);
         u2=repmat(u(r+1,1:n(r+1)),1,col2); 
         for i=r+2:S
            col2=col2/n(i);
            ut=repmat(u(i,1:n(i)),row2,col2);
            u2=u2+reshape(ut,1,collength);
            row2=row2*n(i);
         end	
         u3=repmat(u2,rowlength,1);
         d2=c2-u3;  
         
         [d3 di]=min(d2');
         d3=d3';
         d=reshape(d3,n(1:r));
          
		   clear c2 u2 u3 d2 d3;   
      
   
         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
         % STEP 3: Enforce constraint set r. r=2,3,...,S
         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
         % !!!! some work need to do for index (index(r))?
         d2=reshape(d,prod(n(1:r-1)),n(r));
         for i=1:n(1)
             index=gam(i,r-1)-1;		 % gam(i,1) = i
            for j=r-2:-1:1
               index=index*n(j)+gam(i,j)-1;
            end   
            index=index+1;
            dr(i,1:n(r))=d2(index,1:n(r));
         end   
         
         [J(r),omiga,assign]=auction(-dr);  %  dr is always a two-dim (n1 X n(r)) matrix
         J(r)=-J(r)+sum(sum(u(r+1:S,:)));
         clear dr;
         assi = find_assi_index(assign, n(1));
         gam=[gam assi];
         
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      % STEP 4: Update Lagrangian Multiplers u for r=3,4,...,S.
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      if r==2
          Jstar=J(2);   
          f_dual=max(f_dual,Jstar);
      else    
         g(r,1:n(r))=ones(1,n(r));
         for k=1:n(1)
            index=gam(k,r-1)-1;		 % gam(k,1) = k
            for i=r-2:-1:1
               index=index*n(i)+gam(k,i)-1;
            end
            index=index+1;
            j=mod(do(index)-1,n(r))+1; % ????
            g(r,j)=g(r,j)-1;
         end
         
         % find the adapative factor for the updating of u
         % Accelerated Subgradient Update:
         % adapt=eye(n(r));
         % u(r,1:n(r))=u(r,1:n(r))+g(r,1:n(r))*adapt;
         if g(r,1:n(r))==0
         else   
            if mod(iter,n(r))==0
               p=zeros(n(r),1);
               H2=eye(n(r));
            else
               H2=H(1:n(r),1:n(r),r);
               p=H2*g(r,1:n(r))';
               H2=H2+(1-alpha^(-2))*p*p'/(g(r,1:n(r))*p);
            end   
            fa_dual=(1+a/beta^b)*f_dual;
            adapt=(alpha+1)/alpha*(J(r)-fa_dual)/norm(g(r,1:n(r)))/norm(g(r,1:n(r)));
            u(r,1:n(r))=u(r,1:n(r))+p'*adapt;
            if J(2)<f_dual
               beta=beta+1;
            else
               beta=max(beta-1,1);
            end   
            H(1:n(r),1:n(r),r)=H2;
         end 
      end
      do=di;
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      % STEP 5: Recursion: Identify assignments for the R-D problem
      %         Increment R and go to the STEP 3.
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
           
   end % r or R
   
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % STEP 6: Iteration: Improve solution quality, go to STEP 2 or STEP 7.
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%\
   % for r=S case.
   d2=reshape(c,prod(n(1:S-1)),n(S));
   for i=1:n(1)
      index=gam(i,S-1)-1;		 % gam(i,1) = i
      for j=S-2:-1:1
         index=index*n(j)+gam(i,j)-1;
      end
      index=index+1;
      if index > 0
          dr(i,:)=d2(index,1:n(S));
      end
   end   
   
   [J(S),omiga,assign]=auction(-dr);  %  dr is always a two-dim (n1 X n(r)) matrix
   J(S)=-J(S);
   clear dr;

   assi = find_assi_index(assign, n(1));
   gam=[gam assi];
   
   % update multiplier u(S,:)
   g(S,1:n(S))=ones(1,n(S));
   for k=1:n(1)
      index=gam(k,S-1)-1;		 % gam(k,1) = k
      for i=S-2:-1:1
         index=index*n(i)+gam(k,i)-1;
      end
      index=index+1;
      if index > 0
          j=mod(do(index)-1,n(S))+1; % ????
          g(S,j)=g(S,j)-1;
      end
   end
   
   if g(S,1:n(S))==0
   else   
      if mod(iter,n(S))==0
         p=zeros(n(S),1);
         H2=eye(n(S));
      else  
         H2=H(1:n(S),1:n(S),S);
         p=H2*g(S,1:n(S))';
         H2=H2+(1-alpha^(-2))*p*p'/(g(S,1:n(S))*p);
      end   
      fa_dual=(1+a/beta^b)*f_dual;
      adapt=(alpha+1)/alpha*(J(S)-fa_dual)/norm(g(S,1:n(S)))/norm(g(S,1:n(S)));
      u(S,1:n(S))=u(S,1:n(S))+p'*adapt;
      if J(2)<f_dual
         beta=beta+1;
      else
         beta=max(beta-1,1);
      end   
      H(1:n(S),1:n(S),S)=H2;
   end   
      
  % Jstar=J(2);   
  % f_dual=max(f_dual,Jstar);
   f_primal=min(f_primal,J(S));
   rgap=(f_primal-f_dual)/abs(f_primal);
   iter=iter+1;
   
end  % while

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STEP 7: Final Results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% get the last assignment result, make it easy to find obj(s(i)) for person(i)

%assignlast=gam(:,2);
%assignlast2=gam(:,3);
%[temp, assignlast]=sort(assign);
%[temp, assignlast2]=sort(assign2);
%assignlast=assignlast(1+n2_FA:n2);  
%assignlast2=assignlast2(1+n3_FA:n3);