WSN定位蒙特卡洛方法MCL的MATLAB实现源码

clear;clc;%初始化工作Ns=20;Nn=200;Vmax=20;Xrange=200;Yrange=200;tr=50;step=20;N=20;Nf=3;%采样盒子确定时，估计位置要扩大圆面积ns_range=200;%每个采样盒子的最大采样次数fori=1:NsXseed(1,i)=rand(1,1)*Xrange;Yseed(1,i)=rand(1,1)*Yrange;endfori=1:NnXnode(1,i)=rand(1,1)*Xrange;Ynode(1,i)=rand(1,1)*Yrange;Xnode_g(1,i)=Xnode(1,i);%MCL估计位置,初始值设置为真实位置Ynode_g(1,i)=Ynode(1,i);end%初始时刻的粒子群，foreverynodefori=1:Nnforj=1:Nlx(i,j,1)=Xnode_g(1,i);ly(i,j,1)=Ynode_g(1,i);endend%figure(1);%plot(Xseed,Yseed,'bo',Xnode,Ynode,'k*');%节点们开始运动，每次定位完成才开始下一次运动，这里假设这个定位过程耗时非常短%仿真步数fork=2:step%新的时刻，节点们先运动一下,RWP模型fori=1:Nsr=rand(1,1)*Vmax;thita=rand(1,1)*2*pi;Xseed(k,i)=Xseed(k-1,i)+r*cos(thita);ifYnode(k,i)Yrange||Ynode(k,i)0Xnode(k,i)=Xnode(k-1,i)+r*cos(thita);Ynode(k,i)=Ynode(k-1,i)-r*sin(thita);endend%对每一个node逐个进行定位fori=1:Nn%测试每个seed是否可以用来定位A1=[];%存储1跳锚节点序号A2=[];%存储2跳锚节点序号forj=1:Nsd1=sqrt((Xnode(k,i)-Xseed(k,j))^2+(Ynode(k,i)-Yseed(k,j))^2);ifd1=trA1=[A1j];endifd1tr&&d1=2*trform=1:Nnifm~=idmi=sqrt((Xnode(k,i)-Xnode(k,m))^2+(Ynode(k,i)-Ynode(k,m))^2);dms=sqrt((Xnode(k,m)-Xseed(k,j))^2+(Ynode(k,m)-Yseed(k,j))^2);ifdmi=tr&&dms=trA2=[A2j];endendendendend%接下来要获取采样盒子，每个粒子都要有一个采样盒子，然后还要进行采样，每个采样盒子的采样都要有一个次数限制，这里还是要分四种情况temp1=size(A1,2);temp2=size(A2,2);%第一种情况iftemp1==0&&temp2==0Xnode_g(k,i)=Xnode_g(k-1,i);Ynode_g(k,i)=Ynode_g(k-1,i);forj=1:Nlx(i,j,k)=0;%周围没有锚节点，没法进行定位，则粒子群全部置零ly(i,j,k)=0;endend%第二种情况iftemp1~=0&&temp2==0forj=1:Nsambox1=[Xnode_g(k-1,i)-Nf*Vmax];sambox2=[Xnode_g(k-1,i)+Nf*Vmax];sambox3=[Ynode_g(k-1,i)-Nf*Vmax];sambox4=[Ynode_g(k-1,i)+Nf*Vmax];Xsambox_min(i,j,k)=max(sambox1);Xsambox_max(i,j,k)=min(sambox2);Ysambox_min(i,j,k)=max(sambox3);Ysambox_max(i,j,k)=min(sambox4);%从采样盒子里采样lx(i,j,k)=unifrnd(Xsambox_min(i,j,k),Xsambox_max(i,j,k),1,1);ly(i,j,k)=unifrnd(Ysambox_min(i,j,k),Ysambox_max(i,j,k),1,1);ns=1;%滤波and=[];form=1:temp1d1(m)=sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);endendns=ns+1;end%进行最终的判断and=[];form=1:temp1d1(m)=sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);ifd1(m)=trand=[and0];endifd1(m)trand=[and1];endendifsum(and)~=0lx(i,j,k)=0;ly(i,j,k)=0;endend%至此节点i的粒子群已经得到%下面是进行定位，粒子群的优化num_zero=0;forj=1:Niflx(i,j,k)==0num_zero=num_zero+1;endend%k时刻的定位位置ifnum_zero==NXnode_g(k,i)=Xnode_g(k-1,i);Ynode_g(k,i)=Ynode_g(k-1,i);endifnum_zero~=NXnode_g(k,i)=sum(lx(i,:,k))/(N-num_zero);Ynode_g(k,i)=sum(ly(i,:,k))/(N-num_zero);end%粒子群补全N个ifnum_zero==Nforj=1:Nlx(i,j,k)=0;ly(i,j,k)=0;endendifnum_zero~=Nforj=1:Niflx(i,j,k)==0lx(i,j,k)=Xnode_g(k,i);ly(i,j,k)=Ynode_g(k,i);endendendend%第三种情况iftemp1==0&&temp2~=0forj=1:Nsambox1=[Xnode_g(k-1,i)-Nf*Vmax];sambox2=[Xnode_g(k-1,i)+Nf*Vmax];sambox3=[Ynode_g(k-1,i)-Nf*Vmax];sambox4=[Ynode_g(k-1,i)+Nf*Vmax];Xsambox_min(i,j,k)=max(sambox1);Xsambox_max(i,j,k)=min(sambox2);Ysambox_min(i,j,k)=max(sambox3);Ysambox_max(i,j,k)=min(sambox4);and=[and0];elseand=[and1];endendns=ns+1;end%进行最终的判断and=[];form=1:temp2d1(m)=sqrt((lx(i,j,k)-Xseed(k,A2(m)))^2+(ly(i,j,k)-Yseed(k,A2(m)))^2);ifd1(m)tr&&d1(m)=2*trand=[and0];elseand=[and1];endendifsum(and)~=0lx(i,j,k)=0;ly(i,j,k)=0;endend%至此节点i的粒子群已经得到%下面是进行定位，粒子群的优化num_zero=0;forj=1:Niflx(i,j,k)==0num_zero=num_zero+1;endend%k时刻的定位位置ifnum_zero==NXnode_g(k,i)=Xnode_g(k-1,i);Ynode_g(k,i)=Ynode_g(k-1,i);endifnum_zero~=NXnode_g(k,i)=sum(lx(i,:,k))/(N-num_zero);Ynode_g(k,i)=sum(ly(i,:,k))/(N-num_zero);end%粒子群补全N个ifnum_zero==Nforj=1:Nlx(i,j,k)=0;ly(i,j,k)=0;endendifnum_zero~=Nforj=1:Niflx(i,j,k)==0lx(i,j,k)=Xnode_g(k,i);ly(i,j,k)=Ynode_g(k,i);endendendend%第四种情况iftemp1~=0&&temp2~=0forj=1:Nsambox1=[Xnode_g(k-1,i)-Nf*Vmax];sambox2=[Xnode_g(k-1,i)+Nf*Vmax];sambox3=[Ynode_g(k-1,i)-Nf*Vmax];sambox4=[Ynode_g(k-1,i)+Nf*Vmax];Xsambox_min(i,j,k)=max(sambox1);Xsambox_max(i,j,k)=min(sambox2);Ysambox_min(i,j,k)=max(sambox3);Ysambox_max(i,j,k)=min(sambox4);%从采样盒子里采样lx(i,j,k)=unifrnd(Xsambox_min(i,j,k),Xsambox_max(i,j,k),1,1);ly(i,j,k)=unifrnd(Ysambox_min(i,j,k),Ysambox_max(i,j,k),1,1);ns=1;%滤波and=[];form=1:temp1d1(m)=sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);ifd1(m)=trand=[and0];endifd1(m)=trand=[and0];endifd1(m)trand=[and1];endd1(m)=sqrt((lx(i,j,k)-Xseed(k,A2(m)))^2+(ly(i,j,k)-Yseed(k,A2(m)))^2);ifd1(m)tr&&d1(m)=2*trand=[and0];elseand=[and1];endendifsum(and)~=0lx(i,j,k)=0;ly(i,j,k)=0;endend%至此节点i的粒子群已经得到%下面是进行定位，粒子群的优化num_zero=0;forj=1:Niflx(i,j,k)==0num_zero=num_zero+1;endend%k时刻的定位位置ifnum_zero==NXnode_g(k,i)=Xnode_g(k-1,i);Ynode_g(k,i)=Ynode_g(k-1,i);endifnum_zero~=NXnode_g(k,i)=sum(lx(i,:,k))/(N-num_zero);Ynode_g(k,i)=sum(ly(i,:,k))/(N-num_zero);end%粒子群补全N个ifnum_zero==Nforj=1:Nlx(i,j,k)=0;ly(i,j,k)=0;error_total(k)=sum(error(k,:))/Nn;endk=1:size(error_total,2)plot(k,error_total,'b-o');