multilateration_temporarily.m

加注释的multilateration_temporarily 无线传感网络定位

function multilateration(M,N,delta,R,option)
%M为锚节点的个数，N为非锚节点的个数，delta为标准差,用于比较multilateration算法与下限的差距
%option用来选择是否在每一步计算之后采用迭代算法，option＝0为不选择，option＝1为选择
r=random('unif',0,1,N+M,1);
r=sqrt(r);
theta=random('unif',0,2*pi,N+M,1);
x=r.*cos(theta);
y=r.*sin(theta);
X=[x,y];
Y=zeros(N+M,2);%Y是估计的坐标矩阵
Z=zeros(N+M,2);%Z是采用迭代法估计的坐标矩阵
Y(1:M,:)=X(1:M,:);%Y的前M行表示锚节点坐标
Z(1:M,:)=X(1:M,:);%Z的前M行表示锚节点坐标
D=noise_distance(X,1,delta,2);%生成距离矩阵,验证点与未知节点的混合距离矩阵，noise_distance是距离计算函数，也可以生成受高斯噪声干扰的测量距离矩阵
D_0=noise_distance(X);
tic;
temp_matrix=D_0(:,1:M);%不应该以噪声矩阵来判断距离，而应该以无噪声的矩阵来判断
w=zeros(N+M,1);
w(1:M,:)=1;
z1=zeros(N+M,1);
z2=zeros(N+M,1);
z2(1:M,:)=1;
flag=zeros(N,1);
for i=1:N
    for j=1:M
        if norm(X(i+M,:)-X(j,:))<=R
            flag(i)=flag(i)+1;
        end
    end
end
mark=find(flag>=3);%找出哪些节点可以实现与3个或3个以上的锚节点进行通信
w(mark+M)=1;%对可以初始定位的点进行估计
%%%%%%%%%%下面是定位算法中第一步计算的过程
for l=1:length(mark)
    i=mark(l);
    temp=find(temp_matrix(i+M,:)<=R);
    coordinate=joint_equation(X(temp,:),D(temp,i+M));%%%该函数可以实现多点定一点方程的计算
    Y(i+M,1)=coordinate(1);
    Y(i+M,2)=coordinate(2);
end
if option==1
    conect_matrix=zeros(size(find(w),1),size(find(w),1));
    conect_node=find(w);%用于指定哪些节点被迭代，其中锚节点也被包括其中
    conect_matrix(conect_node,conect_node)=1;
    %[z,v]=steepest_decent(D(conect_node,conect_node),Y(conect_node,:),conect_matrix);
    [z,v]=steepest_decent(D(conect_node,conect_node),Y(conect_node,:),conect_matrix);
    Z(conect_node,:)=z;
end
disp(toc);
%%%%%%%%%%%下面是画出节点图，黑点表示锚节点，蓝点表示真实坐标，红点表示估计坐标
figure;
hold on;
for i=1:M
    plot(X(i,1),X(i,2),'k.');
    s=num2str(i);
    text(X(i,1)+0.01,X(i,2)+0.01,s);
end
for i=1:N
    plot(X(i+M,1),X(i+M,2),'b.');
    s=num2str(i);
    text(X(i+M,1)+0.01,X(i+M,2)+0.01,s);
end
for i=1:length(mark)
    plot(Y(mark(i)+M,1),Y(mark(i)+M,2),'r.');
    plot([X(mark(i)+M,1),Y(mark(i)+M,1)],[X(mark(i)+M,2),Y(mark(i)+M,2)],'r-');
    %     s=num2str(mark(i));
    %     text(Y(mark(i)+M,1)-0.03,Y(mark(i)+M,2)-0.03,s);
end