WSN定位蒙特卡洛方法MCL的MATLAB

1、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估计位置,初始值设置

2、为真实位置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%新的时亥I，节点们先运动一下,RWP模型fori=1:Nsr=rand(1,1)*Vmax;thita=r

3、and(1,1)*2*pi;Xseed(k,i)=Xseed(k-1,i)+r*cos(thita);沟旦g-dLK-¥s£ed(k-lri+rTsi号肺氏止出界，誇要增；IIIIJ下M折顷|£11線痢如wxseedjU)=xseedfk-l.i)-roosittiitat:eed(kTi=Yseedfk-lj)+r*5in(thita);+J-nr-nr-nr-nr-riiJrr.endn1ifVseedfLi)>VrangeIIeedikji)<0Xseedilkj二Xseedfk-l+严cciRthiit?;迪蔗她J)=烁凶代-1-rI%坦功;+

4、gjod+'.end+'fori=liNnr=randl1l*VTnax：4JtYte-ancljlJSpi;-1泅姒KJ=Knoste(i<-i.i)十严磁加旦);PMjMKaD二madejk-Ui+广引m(血咗;屮鹘找防止出界，看耍增M2T軌折项ifXnodelkJ>KraQESIXn3de(kJ<0卫也0电(KJ=Xr。血曲-训-r*costhita；H|tjBjusjusjusjusv.raaaaaaaa'L1/'4亠4亠?LiAAftAnwr1VTodeikJ)=¥iiode(k-l±i+r*tiifthita;e

5、n.d*1ifYnode(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:Nsdi=sqrt(Xnode(k,i)-Xseed(k,j)A2+(Ynode(k,i)-Yseed(k,j)A2);ifd1<=trA1=A1j;end%接下来要获取采样盒子，每个粒子都要有一

6、个采样盒子，然后还要进行采样，每个采样盒子的采样都要有一个次数限制，这里还是要分四种情况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;s

7、ambox2=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)=

8、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)A2+(ly(i,j,k)-Yseed(k,A1(m)A2);ifdl(im)=trand=andDl;+'WWWVJ*pend*1ifdlfmjtr*-1and-and1；pen.d1hile(5um(andll=D&&ns"=ns_jange+-'哄丛堆样卷子里朵样Hx(i.jik)=unifrndfXsairinbO'Xm

9、in(i.i.kl.XsaunboxifmaxfijLkkl.ll:!V(kLk=unifrjndfysambox_nri.lin.(LLk),Ys旦国口婪_叩無(以町丄丄；3%、虑便and-;*frm=l!innpl+ldl(m_田应山禹占卜龜叙Al(m)忖-(fe(idJs-Yeed(砒)*2)严and=andO'H、mLa1,1endifdl(nn)>t-and=and1:'mu、"«"endendns=ns+1;end%进行最终的判断and=;form=1:temp1d1(m)=sqrt(lx(i,j,k)-Xseed(k,A1(m)

10、A2+(ly(i,j,k)-Yseed(k,A1(m)A2);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_

11、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)

12、-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);验丛痴丰盒子亘采祥IkiLi,k=un_frndIXsambaxmiinfLijk-.Ksambokmax|i.j.k-!.!):*-1IW

13、k二unifrndf¥5amboxminfi.i.klPYsaimbaxmaxfLLkJ-lil；US=1；*肓虎披and-fo;m=i:tennp2Pgnn)=测感叙(kA2(m)严2+迦(站-畑grff灯2(m)户囚;屮ifdl(T<?>t-&&dlml<-2*tri'Lrsrand=and01>'vvwwpe>e+-1and=and1：end*1ejid*-1wt)Msum(and"=0&8lns"=ns_ran£e)+'嶋从采杆莹子旦霁样Ixilifk-unifrndX

14、samboxmin(i-j,k.X5ambokmaxfu.kj.1,1Ivtl.Llc)=unlfmdrft日mbrntmlnlu.kmax儿Lk)rl,1)沪forrm=l;tennp2vdl(m)fdL(m)>tr&&dl(m'<2±rLrvmLrand=and0;elseand=and1;endendns=ns+1;end%进行最终的判断and=;form=1:temp2d1(m)=sqrt(lx(i,j,k)-Xseed(k,A2(m)A2+(ly(i,j,k)-Yseed(k,A2(m)A2);ifd1(m)>tr&&

15、;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_ze

16、ro);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;sa

17、mbox3=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),Ysamb

18、ox_max(i,j,k),1,1);ns=1;%滤波and=;form=1:temp1d1(m)=sqrt(lx(i,j,k)-Xseed(k,A1(m)A2+(ly(i,j,k)-Yseed(k,A1(m)A2);ifd1(m)<=trand=and0;endand=and1；pendk1io;nn-Lemp2Jdl(nnrdi(m)>tr&&di|mK=2*trand=and0:ejlseand=and11;+'endericwhlle5Lm|and=0&&ns=.圧_迢巒屮珀从死样盒子里崇冲亠x(ijjk)=urtifnrKiDcsa

19、imbax耐时汕口麴旦鱼塑一一朗時打丄口1,1;+=iLinifrind(¥sanriboxuninfLj.k),VsaimboxmaxILj.kLl.1!j+J蕊甘and=;fornJUpiP曲巾=融(Ik血打-渥昶砧1竹严2-壯你占卜滋國kALmF2)屮ifd1(m)<=trand=and0;endifd1(m)>trand=and1;enden_d''form=l:temp2+'(加以幻-绥頭化少可呵)严*阳iu卜烝旦或z囚呵产可出ifdliimptr应&Ol(m<xrwLIwvLrwv址迫=and0艸else*1and=faind1;*end-1ns=ns41；+Jend*1:片存缪的艸z宝迫=】;*form二llempl1叫呵=咖阿讹卜净靱*ALm)戶24.刼血卜軽或砧1的)7);屮ifand=and0；i-'艶aifdl(mW*Jand=and1:end-1eirid+lform二I:t6mp2d1(m)sqrt(lx(i,j,k)-Xseed(k,A2(m)A2+(ly(i,j,k)-Yseed(k,A2(m)A2);ifd1(m)>tr&&d1(m)<=2*trand=and0;elseand=and1;endendifsum(and)=0lx(