single_mds_localization.m

WSN中DOA算法的mds定位

function Y=single_MDS_localization(X,D,N,M)
%利用MDS方法计算估计的坐标，N为非锚节点个数，M为锚节点个数
if nargin==1
    D=noise_distance_matrix(X,1);
    DD=noise_distance_matrix(X,2);
    for i=1:3
        for j=1:3
            D(i,j)=DD(i,j);
        end
    end
    D2=D.^2;
    B=zeros(13,13);
    for i=1:13
        for j=1:13
            B(i,j)=-0.5.*D2(i,j)+0.5.*(mean(D2(i,:))+mean(D2(:,j)))-0.5.*mean(mean(D2));
        end
    end
    [U,A]=eig(B);
    Y=U(:,12:13)*(A(12:13,12:13).^(0.5));
    [alpha,T,r]=rigid_transform(X(1:3,:),Y(1:3,:));
    if r==0
        Y=([cos(alpha),-sin(alpha);sin(alpha),cos(alpha)]*Y'+[T(1)*ones(1,size(Y,1));T(2)*ones(1,size(Y,1))])';
    else
        Y=([cos(alpha),-sin(alpha);sin(alpha),cos(alpha)]*[1,0;0,-1]*Y'+[T(1)*ones(1,size(Y,1));T(2)*ones(1,size(Y,1))])';
    end
end
if nargin==4
    D2=D.^2;
    B=zeros(N+M,N+M);
    for i=1:N+M
        for j=1:N+M
            B(i,j)=-0.5.*D2(i,j)+0.5.*(mean(D2(i,:))+mean(D2(:,j)))-0.5.*mean(mean(D2));
        end
    end
    [U,A]=eig(B);
    Y=U(:,N+M-1:N+M)*(A(N+M-1:N+M,N+M-1:N+M).^(0.5));
    if M==0;
    else
        [alpha,T,r]=rigid_transform(X(1:M,:),Y(1:M,:));
        if r==0
            Y=([cos(alpha),-sin(alpha);sin(alpha),cos(alpha)]*Y'+[T(1)*ones(1,size(Y,1));T(2)*ones(1,size(Y,1))])';
        else
            Y=([cos(alpha),-sin(alpha);sin(alpha),cos(alpha)]*[1,0;0,-1]*Y'+[T(1)*ones(1,size(Y,1));T(2)*ones(1,size(Y,1))])';
        end
    end
end