wsn定位蒙特卡洛盒mcb算法matlab源码

1、clear;clc;%初始化工作ns = 40;nn = 400;vmax = 50;xrange = 500;yrange = 500;tr = 100;rg = 0.5*0;step = 6;n = 50;ns_range = 200; %每个采样盒子的最大采样次数nf = 1.5; %采样盒子确定时，估计位置要扩大圆面积for i = 1:ns xseed(1,i) = unifrnd(rg,(xrange-rg),1,1); yseed(1,i) = unifrnd(rg,(yrange-rg),1,1);endfor i = 1:nn xnode(1,i)=rand(1,1)*xra

2、nge; ynode(1,i)=rand(1,1)*yrange; xnode_g(1,i)=xnode(1,i); %mcl估计位置,初始值设置为真实位置 ynode_g(1,i)=ynode(1,i);end%初始时刻的粒子群，for every nodefor i = 1:nn for j = 1:n lx(i,j,1) = 0; ly(i,j,1) = 0; endendfor i = 1:nn error(1,i) = 0;end%figure(1);%plot(xseed,yseed,bo,xnode,ynode,k*);%节点们开始运动，每次定位完成才开始下一次运动，这里假设这个

3、定位过程耗时非常短%仿真步数for k=2:step %新的时刻，节点们先运动一下,rwp模型 if ynode(k,i) yrange | ynode(k,i) 0 xnode(k,i) = xnode(k-1,i) + r*cos(thita); ynode(k,i) = ynode(k-1,i) - r*sin(thita); end end %对每一个node逐个进行定位 for i = 1:nn %测试每个seed是否可以用来定位 a1 = ; %存储1跳锚节点序号 a2 = ; %存储2跳锚节点序号 for j = 1:ns d1 = sqrt(xnode(k,i)-xseed(k

4、,j)2+(ynode(k,i)-yseed(k,j)2); if d1=xsambox_max(i,j,k) | ysambox_min(i,j,k)=ysambox_max(i,j,k) xsambox_min(i,j,k) = xanbox_min(k,i); xsambox_max(i,j,k) = xanbox_max(k,i); ysambox_min(i,j,k) = yanbox_min(k,i); ysambox_max(i,j,k) = yanbox_max(k,i); end %从采样盒子里采样 lx(i,j,k) = unifrnd(xsambox_min(i,j,k)

5、,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 = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2); if d1(m)=tr and = and 0; end %进行最终的判断 and = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,

6、k)-yseed(k,a1(m)2); if d1(m)tr and = and 1; end end if sum(and)=0 lx(i,j,k) = 0; ly(i,j,k) = 0; end end %至此 节点 i 的粒子群已经得到 %下面是进行定位，粒子群的优化 num_zero = 0; for j = 1:n if lx(i,j,k) = 0 num_zero = num_zero+1; end end %k时刻的定位位置 end %第二种情况 的 上一时间没有锚节点，没有采样粒子的更新，那么这种情况下 采样盒子就是锚盒子 if temp1=0 & temp2=0 & sum(

7、lx(i,:,k-1)=0 for j = 1:n sambox1 = xanbox_min(k,i) ; sambox2 = xanbox_max(k,i) ; sambox3 = yanbox_min(k,i) ; sambox4 = yanbox_max(k,i) ; 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) =

8、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 = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2); if d1(m)tr and = and 1; end end while(sum(and)=0 & ns = ns_range) %从采样盒子里采样 lx(i,j

9、,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); %滤波 and = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2); if d1(m)tr and = and 1; end end ns = ns+1; end %进行最终的判断 and = ; for m = 1:temp1 d1(m

10、) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2); if d1(m)tr and = and 1; end end if sum(and)=0 lx(i,j,k) = 0; ly(i,j,k) = 0; end end %至此 节点 i 的粒子群已经得到 %下面是进行定位，粒子群的优化 num_zero = 0; for j = 1:n if lx(i,j,k) = 0 num_zero = num_zero+1; end end %k时刻的定位位置 if num_zero = n xnode_g(k,i) = xnode

11、_g(k-1,i); ynode_g(k,i) = ynode_g(k-1,i); end if num_zero = n xnode_g(k,i) = sum(lx(i,:,k)/(n-num_zero); ynode_g(k,i) = sum(ly(i,:,k)/(n-num_zero); end %粒子群补全n个 if num_zero = n for j = 1:n lx(i,j,k) = 0; ly(i,j,k) = 0; end end if num_zero = n for j = 1:n if lx(i,j,k) = 0 lx(i,j,k) = xnode_g(k,i); ly

12、(i,j,k) = ynode_g(k,i); end end end end %第三种情况 的 上一时间有锚节点，也就是有采样粒子的更新 if temp1=0 & temp2=0 & sum(lx(i,:,k-1)=0 for j = 1:n sambox1 = xanbox_min(k,i) xnode_g(k-1,i)-nf*vmax; sambox2 = xanbox_max(k,i) xnode_g(k-1,i)+nf*vmax; sambox3 = yanbox_min(k,i) ynode_g(k-1,i)-nf*vmax; sambox4 = yanbox_max(k,i) y

13、node_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); %增加一步判断项，若是锚盒子和运动圆能够有交集，就使用交集，如果没有交集，则使用锚盒子 if xsambox_min(i,j,k)=xsambox_max(i,j,k) | ysambox_min(i,j,k)=ysambox_max(i,j,k) xsambox_min(

14、i,j,k) = xanbox_min(k,i); xsambox_max(i,j,k) = xanbox_max(k,i); ysambox_min(i,j,k) = yanbox_min(k,i); ysambox_max(i,j,k) = yanbox_max(k,i); end %从采样盒子里采样 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 =

15、; for m = 1:temp2 d1(m) = sqrt(lx(i,j,k)-xseed(k,a2(m)2+(ly(i,j,k)-yseed(k,a2(m)2); if d1(m)tr & d1(m)tr & d1(m)tr & d1(m)tr & d1(m)tr & d1(m)=xsambox_max(i,j,k) | ysambox_min(i,j,k)=ysambox_max(i,j,k) xsambox_min(i,j,k) = xanbox_min(k,i); xsambox_max(i,j,k) = xanbox_max(k,i); ysambox_min(i,j,k) = y

16、anbox_min(k,i); ysambox_max(i,j,k) = yanbox_max(k,i); end %从采样盒子里采样 %从采样盒子里采样 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); %滤波 and = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2);

17、if d1(m)tr and = and 1; end end for m = 1:temp2 d1(m) = sqrt(lx(i,j,k)-xseed(k,a2(m)2+(ly(i,j,k)-yseed(k,a2(m)2); if d1(m)tr & d1(m)=2*tr and = and 0; else and = and 1; end end ns = ns+1; end %进行最终的判断 and = ; for m = 1:temp1 d1(m) = sqrt(lx(i,j,k)-xseed(k,a1(m)2+(ly(i,j,k)-yseed(k,a1(m)2); if d1(m)tr and = and 1; end end for m = 1:temp2 d1(m) = sqrt(lx(i,j,k)-xseed(k,a2(m)2+(ly(i,j,k)-yseed(k,a2(m)2); if d1(m)tr & d1(m)=2*tr and = and 0