slr.m

来自「SLR 定位算法的MATLAB实现,针对2006 IEEE中的一篇文章写的,实现」· M 代码 · 共 310 行

% 2008.1.17 开始SL_R 定位了Num个未知节点--2008.1.20结束
% Author: jason_deng
% Instrutor:: Liu Hao
% 初始部分花时13秒左右
% 优化部分花时80秒左右
% 硬件环境: Core 2 4400  双核2.0GHz, RAM: DDRII667 HD:160G SATA
% 软件环境: windowsxp SP2 MATLAB7.0.4
clear all
tic
%产生100个随机节点node(x(i),y(i))
Num=210;
%产生的节点所在区域大小为10*10的正方形区域
area=500;
%锚节点数10个
AnchorNum=90;
%锚节点变量AnchorX,AnchorY
%随机生成Num个未知节点
figure(1)
for i=1:Num
     temp=area*rand(1,2);
     node(i,1)=temp(1); %x
     node(i,2)=temp(2); %y
     node1(i,1)=node(i,1);
     node1(i,2)=node(i,2);
     text(node(i,1) ,node(i,2) ,num2str(i),'FontSize',5);
end
%随机生成AnchorNum个锚节点
for i=1:AnchorNum
    temp=area*rand(1,2);
    anchor(i,1)=temp(1);    %x
    anchor(i,2)=temp(2);    %y
end
%画图
hold on
plot(node(:,1),node(:,2),'rO');% 红圈
plot(anchor(:,1),anchor(:,2),'k^','MarkerFaceColor','k'); %上三角形填充色黑色

%SL-R算法开始实现......
% 未知节点一跳内与锚节点的距离为Rn_A,先申明这个量

% EPXNO 即距离的差值
EPXNO=5;
%选点用于计算估计节点个数N
N=50
%节点的通信半径
Radio=50.0;%即一跳
%寻找未知节点一跳内的某个锚节点
count=0; %记录一跳内未找到锚节点的未知节点的计数
count2=0;%记录一跳到二跳间的未找到锚节点的未知节点的计数
for i=1:Num
    Flag=1;
    for j=1:AnchorNum       
        Rn_A(i)=sqrt((node(i,1)-anchor(j,1))^2 + (node(i,2)-anchor(j,2))^2); %R13
        if Rn_A(i)<=Radio
              Anchor_1(i,1)=anchor(j,1); %未知节点一跳内的一个锚节点
              Anchor_1(i,2)=anchor(j,2);
              Flag=0;
              break;
        end
    end
    if Flag==1
        %一跳找不到的未知节点,留后处理,目前采用丢弃处理%后面处理除去这些节点(不为之定位)
        count=count+1;
        RecordNoOneHop(count)=i;
        %做标记
        node(i,1)=-1;
    else
        % 寻找未知节点二跳内的某个锚节点
        Flag2=1;
        for j=1:AnchorNum
            Rn_A2=sqrt((node(i,1)-anchor(j,1))^2 + (node(i,2)-anchor(j,2))^2); %R13.引入40%的误差
            if Rn_A2<=2*Radio & Rn_A2 > Radio
               Anchor_2(i,1)=anchor(j,1);%未知节点二跳内的一个锚节点
               Anchor_2(i,2)=anchor(j,2);
               Flag2=0;
               break;
            end
        end
        if Flag2==1
        %两跳找不到的未知节点,留后处理
           count2=count2+1;
           RecordNoTwoHop(count2)=i;
        %后面处理除去这些节点(不为之定位)
           node(i,1)=-1;
        end
    end  
end

%随机产生5000个点,根据条件找到估计节点坐标
randNum=2000;
countN=100;

for i=1:Num
    %除去不符合要求的节点,不予定位
    if ( node(i,1)==-1)
        continue;
    end
    nodes(i,1)=0;%为后面优化用时的初始化
    countnum=0;
    ex(i,1)=0;
    ex(i,2)=0;
    for j=1:randNum
        tempR=rand(1);
        tempA=6.28*rand(1);
        randnode(j,1)=Radio*tempR*cos(tempA)+Anchor_1(i,1);
        randnode(j,2)=Radio*tempR*sin(tempA)+Anchor_1(i,2);
         if (randnode(j,1)>=500)|( randnode(j,2)>=500) |( randnode(j,1)< 0 ) | randnode(j,2) < 0
             continue;
         end
 
        if ( sqrt( ( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )<=Radio ) ...
        &&( sqrt(( randnode(j,1)-Anchor_2(i,1))^2 +  ( randnode(j,2)-Anchor_2(i,2))^2 )<=2*Radio ) ...
        && abs(Rn_A(i)-abs(sqrt(( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )) )<= EPXNO 
            countnum=countnum+1;
            ex(i,1)=ex(i,1)+randnode(j,1);
            ex(i,2)=ex(i,2)+randnode(j,2);
         
        end
        if countnum==countN
            break;
        end
    end
    countnum;
    if (countnum)
       ex(i,1)=ex(i,1)/countnum;
       ex(i,2)=ex(i,2)/countnum;
      
    else % 找不到时
       %ex(i,1)=node(i,1);
       %ex(i,2)=node(i,2);
       node(i,1)=-1;
    end
end
 %显示运行状态
count
size(ex(:,1))
size(node(:,1))
size(Anchor_1)
size(Anchor_2)
%输出初始处理后的结果
rangebaseerr=0;
rberrcount=0;
for i=1:Num
    if (node(i,1)==-1)
        continue;
    end
   
   plot(ex(i,1),ex(i,2),'bO')%
   
   line([node(i,1) ex(i,1)],[node(i,2) ex(i,2)]);
   % 求定位误差
   tes(i)=abs(sqrt((ex(i,1)-node(i,1))^2+(ex(i,2)-node(i,2))^2))
   rangebaseerr=rangebaseerr+abs(sqrt((ex(i,1)-node(i,1))^2+(ex(i,2)-node(i,2))^2))/Radio;
   rberrcount=rberrcount+1;
end
t1=toc
rangebaseerr=rangebaseerr/rberrcount


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%优化........
tic
Delta=1;
%可处理节点个数
excount=Num-count-count2
%找100个节点
% 先找出邻居节点再算,先存放下每个节点的邻层节点
for i=1:Num
    ci(i)=0;%记i节点的邻居个数
end
Neighbor(Num,1)=0;
for i=1:Num-1
    if ( node(i,1)==-1) 
        continue;
    end
    for j=i+1:Num
        if ( node(j,1)==-1) 
            continue;
        end
        
        distance= sqrt((node(i,1)-node(j,1))^2 + (node(i,2)-node(j,2))^2)*0.55;%计算距离        
        if distance <Radio  %找到邻居
            ci(i)=ci(i)+1 ;%
            Neighbor(i,ci(i))=j;
            
            %同时相对的j也认为i是他的邻居
            ci(j)=ci(j)+1;
            Neighbor(j,ci(j),1)=i;
            
        end
    end
end
% 找节点的对应的一跳内的锚节点
for i=1:Num
    if ( node(i,1)==-1) 
        continue;
    end
    cAnchor(i)=0;%记录i节点的邻居锚节点数
    for j=1:AnchorNum
           distance=sqrt((node(i,1)-anchor(j,1))^2 + (node(i,2)-anchor(j,2))^2); %R13
        if distance <=Radio
            cAnchor(i)=cAnchor(i)+1;
            NeighborAnchor(i,cAnchor(i))=j;
        end
    end
end
toc
tic
while (1)
  for i=1:Num
     if ( node(i,1)==-1)
         continue;
     end
     if ( nodes(i,1)==-2)
        continue;
     end
     countnum=0;
     for j=1:randNum
        tempR=rand(1);
        tempA=6.28*rand(1);
        randnode(j,1)=Radio*tempR*cos(tempA)+Anchor_1(i,1);
        randnode(j,2)=Radio*tempR*sin(tempA)+Anchor_1(i,2);
        if (randnode(j,1)>=500)|( randnode(j,2)>=500)|( randnode(j,1)< 0 ) | randnode(j,2) < 0
             continue;
        end
        %根据公式二获得估计节点
     %  if ( ( ( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )<=2500 ) ...
     %   &( (( randnode(j,1)-Anchor_2(i,1))^2 +  ( randnode(j,2)-Anchor_2(i,2))^2 )<=10000 ) ...
     %   &( Rn_A(i)-EPXNO )^2 < (( randnode(j,1)-Anchor_1(i,1))^2 + ( randnode(j,2)-Anchor_1(i,2))^2 )
            if ( sqrt( ( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )<=Radio ) ...
        &&( sqrt(( randnode(j,1)-Anchor_2(i,1))^2 +  ( randnode(j,2)-Anchor_2(i,2))^2 )<=2*Radio ) ...
        && abs(Rn_A(i)-abs(sqrt(( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )) )<= EPXNO 
      
           countnum=countnum+1;
           RiA(i,countnum)=0;%第i个需定位的节点的第countnum个随机节点的权值
           for k=1:cAnchor(i)
               ik=NeighborAnchor(i,k);
               distance=sqrt((randnode(j,1)-anchor(ik,1))^2 + (randnode(j,2)-anchor(ik,2))^2);%计算距离
               RiA(i,countnum)=RiA(i,countnum)+...
                   1.5*((sqrt((node(i,1)-anchor(ik,1))^2 + (node(i,2)-anchor(ik,2))^2) -  distance)^2);
           end %for k=1:cAnchor(i)
            %找与一跳内的相邻节点
           randnodeiN(i,countnum)=0;
           for k=1:ci(i)
               ik=Neighbor(i,k);
               distance= sqrt((node(i,1)-node(ik,1))^2 + (node(i,2)-node(ik,2))^2);%计算距离
               %判断邻居节点是否在一跳内(除锚节点)
               if distance < Radio  %计算权值
                    randnodeiN(i,countnum)=randnodeiN(i,countnum)+...
                    ( distance  - sqrt((randnode(j,1)-ex(ik,1))^2 + (randnode(j,2)-ex(ik,2))^2))^2;
               end
            end  % for k=1:ci(i)
            w(i,countnum)=RiA(i,countnum)+randnodeiN(i,countnum);
            tex(countnum,1)=randnode(j,1);
            tex(countnum,2)=randnode(j,2);
        end %if ( sqrt( ( randnode(j,1)-Anchor_1(i,1))^2 +  ( randnode(j,2)-Anchor_1(i,2))^2 )<=Radio ) ...
        if countnum==countN
            break;
        end
    end    %for j=1:randNum
  
    %如果5000中一个满足条件的节点也没有找到话
    if ( countnum==0 )
        ext(i,1)=ex(i,1);
        ext(i,2)=ex(i,2);
    else
       %找权值最小的那个随机节点
       tmin=w(i,1);
       posion=1;%获位置值
       for k=2:countnum
           if tmin> w(i,k)
               tmin=w(i,k);
               posion=k;
           end
       end
       ext(i,1)=tex(posion,1);%第n次优化得到估计坐标值
       ext(i,2)=tex(posion,2);   
    end % if ( countnum==0 )
    if  ( sqrt( (ex(i,1)-ext(i,1))^2+(ex(i,2)-ext(i,2))^2) ) < Delta
        nodes(i,1)=-2; %之后这个节点就不再优化了
        excount=excount-1;
    end
    ex(i,1)=ext(i,1);
    ex(i,2)=ext(i,2);
  end   %for i=1:Num
  if excount==0
      break;
  end
end % while (1)
tx=toc
%绘制优化后的结果
figure(2)
%输出初始处理后的结果
rangebaseerr=0;
rberrcount=0;
for i=1:Num
    plot(node1(i,1),node1(i,2),'rO');% 红圈
    if (node(i,1)==-1)
        continue;
    end
    hold on
    plot(ex(i,1),ex(i,2),'b*')%
    line([node(i,1) ex(i,1)],[node(i,2) ex(i,2)]);
      % 求定位误差
   rangebaseerr=rangebaseerr+norm(ex(i,:)-node(i,:) )/Radio;
   rberrcount=rberrcount+1;
end
plot(anchor(:,1),anchor(:,2),'k^','MarkerFaceColor','k');%上三角形填充色黑色
rangebaseerr=rangebaseerr/rberrcount