matrix_optimala.asv

自己做的毕设

function   [a1,segmaX]=matrix_optimala(a,Da,Wa,e)
%%% a(N*K) is the coordinates of the unknown nodes,N:is the number of the
%%% unknown nodes,K: is the dimensional(2 or 3 in fact)
%%% Da(N*N) is the measured distance matrix between the unknown nodes
%%% Wa(N*N) is the weight matrix of the unknown nodes
%%% a1(N*K) is the returned coordinate matrix according to the input a
%%% segmaX is the error value according to the measured distance Da and the
%%% estimated coordinates
%%% Author: Xie Dongfeng
[row_a,column_a]=size(a);
%%% row_a=N
revisedDis=zeros(row_a,row_a);
for i=1:row_a
    for j=1:row_a
        for k=1:column_a
            revisedDis(i,j)=revisedDis(i,j)+DistanceSmooth(a(i,k)-a(j,k),e)^2;
        end
    end
end
 
 segma=0;
 for i=1:row_a
     for j=i+1:row_a
         segma=segma+Wa(i,j)*Da(i,j)*Da(i,j);
     end
 end
 
 V1=zeros(row_a,row_a);
 for i=1:row_a
     for j=1:row_a
         if(i~=j)
             V1(i,j)=-Wa(i,j);
             V1(i,i)=V1(i,i)+Wa(i,j);
         end
     end
 end
 
 alfa1=zeros(row_a,row_a,column_a);
 beta1=zeros(row_a,row_a,column_a);
 P1X=zeros(row_a,column_a);
 lamata0 = 0;
 for i=1:row_a
     for j=1:row_a
         for k=1:column_a
             if(abs(a(i,k)-a(j,k))<e)
                 alfa1(i,j,k) = 3.5-(a(i,k)-a(j,k))^2/(2*e*e);
             else
                 alfa1(i,j,k)=3;
             end
         end
     end
 end
 for i=1:row_a
     for j=1:row_a
         for k=1:column_a
             beta1(i,j,k) = DistanceSmooth((a(i,k)-a(j,k)),e)^2 - 4*(a(i,k)-a(j,k))^2+2*(a(i,k)-a(j,k))^2*alfa1(i,j,k);
         end
     end
 end
 for i=1:row_a
     for k=1:column_a
         for j=1:row_a
             if(j~=i)
                  P1X(i,k)=Wa(i,j)*(a(i,k)-a(j,k))*alfa1(i,j,k);
             end
         end
     end
 end
 
 
 for i=1:row_a
     for j=1:row_a
         TEMP0 = 0;
         for k=1:column_a
              TEMP0 = TEMP0 + beta1(i,j,k);
         end
         lamata0 = lamata0 + Wa(i,j) * TEMP0;
     end
 end
 elta1 = 4*trace(a'*V1*a)-2*trace(a'*P1X)+lamata0;
 
 alfa2=zeros(row_a,row_a,column_a);
 beta2=zeros(row_a,row_a,column_a);
 P2X=zeros(row_a,column_a);
 lamata1 = 0;
 for i=1:row_a
     for j=1:row_a
         for k=1:column_a
             if(abs(a(i,k)-a(j,k))<e)
                 alfa2(i,j,k) = DistanceSmooth((a(i,k)-a(j,k)),e)/e
             else
                 alfa2(i,j,k)=1;
             end
         end
     end
 end
 for i=1:row_a
     for j=1:row_a
         for k=1:column_a
             if(abs(a(i,k)-a(j,k))<e)
                  beta2(i,j,k) = DistanceSmooth((a(i,k)-a(j,k)),e)*((a(i,k)-a(j,k))^2/e-DistanceSmooth((a(i,k)-a(j,k)),e));
             else
                 beta2(i,j,k)=0;
             end
         end
     end
 end
 for i=1:row_a
     for k=1:column_a
         for j=1:row_a
             if(j~=i)
                  P2X(i,k)=Wa(i,j)*Da(i,j)/revisedDis(i,j)*(a(i,k)-a(j,k))*alfa2(i,j,k);
             end
         end
     end
 end
 

 for i=1:row_a
     for j=1:row_a
         TEMP1 = 0;
         for k=1:column_a
              TEMP1 = TEMP1 + beta2(i,j,k);
         end
         lamata1 = lamata1 + Wa(i,j) * Da(i,j)/revisedDis(i,j)*TEMP1;
     end
 end
 elta2 = -2*trace(a'*P2X)+2*lamata1;
 
 for i=1:row_a
     for j=1:row_a
         if(j~=i)
             B1X(i,i)=B1X(i,i)-B1X(i,j);
         end
     end
 end
 
 
 V1a=inv(V1+ones(row_a))-1/(row_a^2)*ones(row_a);
 a1=V1a*B1X*a;
 
 
 segmaX=segma+trace(a'*V1*a)-2*trace(a'*B1X*a);