rangefree6.m

无线传感器网络中一种定位技术APIT协议的修改于实现

function RangeFree6()                  % 函数名称

axis([0,200,0,200]) %设置坐标轴显示范围
%x=[40 80 120 160 200];
x=200*rand(1,100);
y=200*rand(1,100);
A=40;
for m=1:6
for n=1:6
anchor_x((m-1)*6+n)=(m-1)*A;
anchor_y((m-1)*6+n)=(n-1)*A;
end
end
anchor=[anchor_x' anchor_y'];
plot(x,y,'r*',anchor_x,anchor_y,'g*')
xlabel('X轴'); % x轴注解
ylabel('Y轴'); % y轴注解
title('WSN节点分布图'); % 图形标题
%legend('未知节点','锚节点'); % 图形注解
%grid on; % 显示格线
%{ 以下利用三边定位法求未知节点坐标  %}


B=2.5;     %将二维平面化成小格
for m=1:80
for n=1:80
gridiron_x((m-1)*80+n)=(m-1)*B+1.25;
gridiron_y((m-1)*80+n)=(n-1)*B+1.25;
gridiron_flag((m-1)*80+n)=0;
end
end
% plot(gridiron_x,gridiron_y,'g.')   
gridiron=[gridiron_x' gridiron_y' gridiron_flag'];

Location_erro_sum1=0;
Location_erro_sum2=0;
Location_erro_sum3=0;
sumsum=0;
apit_sum_x=0;
apit_sum_y=0;
sum_time=0;
for time=1:1000     %设置定位循环次数
gridiron(:,3)=0;   %把小数据矩阵的标志位全部复位为0
x=200*rand(1,100);
y=200*rand(1,100);
C=[x(100) y(100)];
%C=[190.0805 40.0630];
%apit_x=C(1);
%apit_y=C(2);
for k=1:36
distance(k)=sqrt((C(1)-anchor(k,1))^2+(C(2)-anchor(k,2))^2);
end

[sequen_dis,index]=sort(distance); %对距离进行排序
for mm=1:6
xx(mm)=anchor(index(mm),1);yy(mm)=anchor(index(mm),2);
d(mm)=1.45*sequen_dis(mm);
end
%x1=anchor_x(index(1));y1=anchor_y(index(1));
%x2=anchor_x(index(2));y2=anchor_y(index(2));
%x3=anchor_x(index(3));y3=anchor_y(index(3));
%x4=anchor_x(index(4));y4=anchor_y(index(4));
%d1=1.1*sequen_dis(1);d2=1.1*sequen_dis(2);d3=1.1*sequen_dis(3);
if((xx(2)-xx(1))~=0 & (xx(3)-xx(2))~=0)
   if(((yy(2)-yy(1))/(xx(2)-xx(1)))==((yy(3)-yy(2))/(xx(3)-xx(2))))    %去除3点在一条直线上的点
 swmp_x=xx(3);swmp_y=yy(3);swmp_d=d(3);
 xx(3)=anchor_x(index(4));yy(3)=anchor_y(index(4));d(3)=1.45*sequen_dis(4);
 xx(4)=swmp_x;yy(4)=swmp_y;d(4)=swmp_d;
   end 
elseif((xx(2)-xx(1))==0 & (xx(3)-xx(2))==0)
      swmp_x=xx(3);swmp_y=yy(3);swmp_d=d(3);
      xx(3)=anchor_x(index(4));yy(3)=anchor_y(index(4));d(3)=1.45*sequen_dis(4);
      xx(4)=swmp_x;yy(4)=swmp_y;d(4)=swmp_d;
else
    %空操作
end  

kk=combntns([1 2 3 4 5 6],3);
[kkk,ccc]=size(kk);

for ii=1:kkk
  
       if((xx(kk(ii,2))-xx(kk(ii,1)))~=0 & (xx(kk(ii,3))-xx(kk(ii,2)))~=0)            % 去除3点在一条直线上的点
            if(((yy(kk(ii,2))-yy(kk(ii,1)))/(xx(kk(ii,2))-xx(kk(ii,1))))==((yy(kk(ii,3))-yy(kk(ii,2)))/(xx(kk(ii,3))-xx(kk(ii,2)))))
            break  
            end     %判断未知节点C=[x(50) y(50)]是否在三角形三个点(xx,yy)内部 
       end
       
       if((xx(kk(ii,2))-xx(kk(ii,1)))==0 & (xx(kk(ii,3))-xx(kk(ii,2)))==0)
           break
       end
   
     if(APIT(xx(kk(ii,1)),yy(kk(ii,1)), xx(kk(ii,2)), yy(kk(ii,2)) ,xx(kk(ii,3)), yy(kk(ii,3)), C(1), C(2)))          %AITT测试
         maxmin_xx=[xx(kk(ii,1)) xx(kk(ii,2)) xx(kk(ii,3))];maxmin_yy=[yy(kk(ii,1)) yy(kk(ii,2)) yy(kk(ii,3))];
         xx_min=min(maxmin_xx);xx_max=max(maxmin_xx);yy_min=min(maxmin_yy);yy_max=max(maxmin_yy);
             find_gridiron_index=find((gridiron(:,1)>=xx_min)&(gridiron(:,1)<=xx_max));
             
             for jk=1:numel(find_gridiron_index)
                 find_gridiron(jk,:)=gridiron( find_gridiron_index(jk),:);
                 find_hang(jk)=find_gridiron_index(jk);
             end
               find_gridiron_index2=find((find_gridiron(:,2)>=yy_min)&(find_gridiron(:,2)<=yy_max));
              for mk=1:numel(find_gridiron_index2)
                 find_gridiron2(mk,:)=find_gridiron( find_gridiron_index2(mk),:);
                 find_hang2(mk)=find_hang(find_gridiron_index2(mk));
             %find_gridiron2(mk,4)=find_gridiron(find_gridiron_index2,4);
              end
              
              [nknk,cccc]=size(find_gridiron2);
             for nk=1:nknk
                 if(APIT(xx(kk(ii,1)),yy(kk(ii,1)), xx(kk(ii,2)), yy(kk(ii,2)) ,xx(kk(ii,3)), yy(kk(ii,3)),find_gridiron2(nk,1), find_gridiron2(nk,2)))
                     gridiron(find_hang2(nk),3)=gridiron(find_hang2(nk),3)+1;
                 end
             end
     end
end
   
            find_gridiron_max=max(gridiron(:,3));
             if(find_gridiron_max>=2)
               gridiron_result_index=find(gridiron(:,3)==find_gridiron_max);
               sumsum=numel(gridiron_result_index);
               apit_sum_x=0;
               apit_sum_y=0;
               if(sumsum>=1)
                  for mmm=1:sumsum
                  apit_sum_x=apit_sum_x+gridiron(gridiron_result_index(mmm),1);
                  apit_sum_y=apit_sum_y+gridiron(gridiron_result_index(mmm),2);
                  end 
                  apit_x=apit_sum_x/sumsum;
                  apit_y=apit_sum_y/sumsum ;
                                   
               end
            else
                apit_x=C(1);
                apit_y=C(2);
                sum_time=sum_time+1;              
            end

     
%time
p=0.5*inv([xx(1)-xx(3) yy(1)-yy(3);xx(2)-xx(3) yy(2)-yy(3)])*[xx(1)^2-xx(3)^2+yy(1)^2-yy(3)^2-d(1)^2+d(3)^2;xx(2)^2-xx(3)^2+yy(2)^2-yy(3)^2+d(3)^2-d(2)^2];
p2=[(xx(1)+xx(2)+xx(3))/3 (yy(1)+yy(2)+yy(3))/3];
Location_erro_sum1=Location_erro_sum1+sqrt((p(1)-C(1))^2+(p(2)-C(2))^2);
Location_erro_sum2=Location_erro_sum2+sqrt((apit_x-C(1))^2+(apit_y-C(2))^2);   
Location_erro_sum3=Location_erro_sum3+sqrt((p2(1)-C(1))^2+(p2(2)-C(2))^2);   %质心算法总误差（1000次测量）
end

Location_erro1=Location_erro_sum1/1000
Location_erro2=Location_erro_sum2/(1000-sum_time)
Location_erro3=Location_erro_sum3/1000     %质心算法误差（1000次测量）