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

《wsn定位蒙特卡洛盒mcb算法matlab源碼》由會(huì)員分享，可在線閱讀，更多相關(guān)《wsn定位蒙特卡洛盒mcb算法matlab源碼（21頁(yè)珍藏版）》請(qǐng)?jiān)谘b配圖網(wǎng)上搜索。

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; %每個(gè)采樣盒子的最大采樣次數(shù) Nf = 1.5; %采樣盒子確定時(shí)，估計(jì)位置要擴(kuò)大圓面積 for i = 1:Ns Xseed(1,i) = unifrnd(Rg,(Xrange-Rg),1,1); Yseed(1,i) = unifrnd(Rg,(Yrange-R

2、g),1,1); end for i = 1:Nn Xnode(1,i)=rand(1,1)*Xrange; Ynode(1,i)=rand(1,1)*Yrange; Xnode_g(1,i)=Xnode(1,i); %MCL估計(jì)位置,初始值設(shè)置為真實(shí)位置 Ynode_g(1,i)=Ynode(1,i); end %初始時(shí)刻的粒子群，for every node for i = 1:Nn for j = 1:N lx(i,j,1) = 0; ly(i,j,1) = 0;

3、end end for i = 1:Nn error(1,i) = 0; end %figure(1); %plot(Xseed,Yseed,'bo',Xnode,Ynode,'k*'); %節(jié)點(diǎn)們開始運(yùn)動(dòng)，每次定位完成才開始下一次運(yùn)動(dòng)，這里假設(shè)這個(gè)定位過程耗時(shí)非常短 %仿真步數(shù) for k=2:step %新的時(shí)刻，節(jié)點(diǎn)們先運(yùn)動(dòng)一下,RWP模型 if Ynode(k,i) > Yrange || Ynode(k,i) < 0 Xnode(k,i) = Xnode(k-1,i) + r*cos(

4、thita); Ynode(k,i) = Ynode(k-1,i) - r*sin(thita); end end %對(duì)每一個(gè)node逐個(gè)進(jìn)行定位 for i = 1:Nn %測(cè)試每個(gè)seed是否可以用來定位 A1 = []; %存儲(chǔ)1跳錨節(jié)點(diǎn)序號(hào) A2 = []; %存儲(chǔ)2跳錨節(jié)點(diǎn)序號(hào) for j = 1:Ns d1 = sqrt((Xnode(k,i)-Xseed(k,j)

5、)^2+(Ynode(k,i)-Yseed(k,j))^2); if d1<=tr A1 = [A1 j]; end end if temp1~=0 && temp2==0 a1_x_min = []; a1_x_max = []; a1_y_min = []; a1_y_max = []; for

6、m = 1:temp1 a1_x_min = [a1_x_min Xseed(k,A1(m))-tr]; a1_x_max = [a1_x_max Xseed(k,A1(m))+tr]; a1_y_min = [a1_y_min Yseed(k,A1(m))-tr]; a1_y_max = [a1_y_max Yseed(k,A1(m))+tr]; end Xanbox_min(k,i) = max(a1_x

7、_min); Xanbox_max(k,i) = min(a1_x_max); Yanbox_min(k,i) = max(a1_y_min); Yanbox_max(k,i) = min(a1_y_max); end if temp1==0 && temp2~=0 a2_x_min = []; a2_x_max = []; a2_y_min = []; a2_y_max = [

8、]; for m = 1:temp2 a2_x_min = [a2_x_min Xseed(k,A2(m))-2*tr]; a2_x_max = [a2_x_max Xseed(k,A2(m))+2*tr]; a2_y_min = [a2_y_min Yseed(k,A2(m))-2*tr]; a2_y_max = [a2_y_max Yseed(k,A2(m))+2*tr]; end

9、 Xanbox_min(k,i) = max(a2_x_min); Xanbox_max(k,i) = min(a2_x_max); Yanbox_min(k,i) = max(a2_y_min); Yanbox_max(k,i) = min(a2_y_max); end if temp1~=0 && temp2~=0 a1_x_min = []; a1_x_max = []; a1_y_min = [

10、]; a1_y_max = []; for m = 1:temp1 a1_x_min = [a1_x_min Xseed(k,A1(m))-tr]; a1_x_max = [a1_x_max Xseed(k,A1(m))+tr]; a1_y_min = [a1_y_min Yseed(k,A1(m))-tr]; a1_y_max = [a1_y_max Yseed(k,A1(m))+tr];

11、 end a2_x_min = []; a2_x_max = []; a2_y_min = []; a2_y_max = []; for m = 1:temp2 a2_x_min = [a2_x_min Xseed(k,A2(m))-2*tr]; a2_x_max = [a2_x_max Xseed(k,A2(m))+2*tr]; a2_y_min = [a

12、2_y_min Yseed(k,A2(m))-2*tr]; a2_y_max = [a2_y_max Yseed(k,A2(m))+2*tr]; end a12_x_min = [max(a1_x_min) max(a2_x_min)]; a12_x_max = [min(a1_x_max) min(a2_x_max)]; a12_y_min = [max(a1_y_min) max(a2_y_min)]; a12_y_max =

13、[min(a1_y_max) min(a2_y_max)]; Xanbox_min(k,i) = max(a12_x_min); Xanbox_max(k,i) = min(a12_x_max); Yanbox_min(k,i) = max(a12_y_min); Yanbox_max(k,i) = min(a12_y_max); end % 以上是錨盒子的獲取過程 %接下來要獲取采樣盒子，每個(gè)粒子都要有一個(gè)采樣盒子，然后還要進(jìn)行采樣

14、，每個(gè)采樣盒子的采樣都要有一個(gè)次數(shù)限制，這里還是要分四種情況 %第一種情況 if temp1==0 && temp2==0 Xnode_g(k,i)=Xnode_g(k-1,i); Ynode_g(k,i)=Ynode_g(k-1,i); for j = 1:N lx(i,j,k) = 0; %周圍沒有錨節(jié)點(diǎn)，沒法進(jìn)行定位，則粒子群全部置零 ly(i

15、,j,k) = 0; end end %第二種情況 的 上一時(shí)間有錨節(jié)點(diǎn)，也就是有采樣粒子的更新 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,

16、i)+Nf*Vmax]; sambox3 = [Yanbox_min(k,i) Ynode_g(k-1,i)-Nf*Vmax]; sambox4 = [Yanbox_max(k,i) 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(sam

17、box3); Ysambox_max(i,j,k) = min(sambox4); %增加一步判斷項(xiàng)，若是錨盒子和運(yùn)動(dòng)圓能夠有交集，就使用交集，如果沒有交集，則使用錨盒子 if Xsambox_min(i,j,k)>=Xsambox_max(i,j,k) || Ysambox_min(i,j,k)>=Ysambox_max(i,j,k) Xsambox_min(i,j,k) = Xanbox_min(k,i);

18、 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);

19、 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

20、 and = [and 0]; end %進(jìn)行最終的判斷 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

21、 and = [and 0]; end if d1(m)>tr and = [and 1]; end end if sum(and)~=0 lx(i,j,k) = 0; ly(i,j,k) = 0; end

22、 end %至此 節(jié)點(diǎn) i 的粒子群已經(jīng)得到 %下面是進(jìn)行定位，粒子群的優(yōu)化 num_zero = 0; for j = 1:N if lx(i,j,k) == 0 num_zero = num_zero+1; end end %k時(shí)刻的定位位置

23、 end %第二種情況 的 上一時(shí)間沒有錨節(jié)點(diǎn)，沒有采樣粒子的更新，那么這種情況下 采樣盒子就是錨盒子 if temp1~=0 && temp2==0 && sum(lx(i,:,k-1))==0 for j = 1:N sambox1 = [Xanbox_min(k,i) ]; sambox2 = [Xanbox_max(k,i) ]; samb

24、ox3 = [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); %從

25、采樣盒子里采樣 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 = []; for m = 1:temp1 d1(

26、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 if d1(m)>tr and = [and 1]; end end

27、 while(sum(and)~=0 && ns ~= ns_range) %從采樣盒子里采樣 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 = [];

28、 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 if d1(m)>tr

29、 and = [and 1]; end end ns = ns+1; end %進(jìn)行最終的判斷 and = []; for m = 1:temp1 d1(m) = sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k

30、,A1(m)))^2); if d1(m)<=tr and = [and 0]; end if d1(m)>tr and = [and 1]; end end if sum(and)~=0 lx(i,j,k) = 0;

31、 ly(i,j,k) = 0; end end %至此 節(jié)點(diǎn) i 的粒子群已經(jīng)得到 %下面是進(jìn)行定位，粒子群的優(yōu)化 num_zero = 0; for j = 1:N if lx(i,j,k) == 0 num_zero = num_zero+1;

32、 end end %k時(shí)刻的定位位置 if num_zero == N Xnode_g(k,i) = Xnode_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);

33、 Ynode_g(k,i) = sum(ly(i,:,k))/(N-num_zero); end %粒子群補(bǔ)全N個(gè) if num_zero == N for j = 1:N lx(i,j,k) = 0; ly(i,j,k) = 0; end end if num_zero ~

34、= N for j = 1:N if lx(i,j,k) == 0 lx(i,j,k) = Xnode_g(k,i); ly(i,j,k) = Ynode_g(k,i); end end end end

35、 %第三種情況 的 上一時(shí)間有錨節(jié)點(diǎn)，也就是有采樣粒子的更新 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

36、,i) Ynode_g(k-1,i)-Nf*Vmax]; sambox4 = [Yanbox_max(k,i) 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(samb

37、ox4); %增加一步判斷項(xiàng)，若是錨盒子和運(yùn)動(dòng)圓能夠有交集，就使用交集，如果沒有交集，則使用錨盒子 if Xsambox_min(i,j,k)>=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);

38、 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

39、(i,j,k),1,1); ns = 1; %濾波 and = []; 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]

40、; else and = [and 1]; end end while(sum(and)~=0 && ns ~= ns_range) %從采樣盒子里采樣 lx(i,j,k) = unifrnd(Xsambox_min(i,j,k),Xsambox_max(i,j,k),1,1);

41、 ly(i,j,k) = unifrnd(Ysambox_min(i,j,k),Ysambox_max(i,j,k),1,1); %濾波 and = []; 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 &&

42、 d1(m)<=2*tr and = [and 0]; else and = [and 1]; end end ns = ns+1; end %進(jìn)行最終的判斷 and = [];

43、 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

44、 end if sum(and)~=0 lx(i,j,k) = 0; ly(i,j,k) = 0; end end %至此 節(jié)點(diǎn) i 的粒子群已經(jīng)得到 %下面是進(jìn)行定位，粒子群的優(yōu)化 num_zero = 0; for j =

45、1:N if lx(i,j,k) == 0 num_zero = num_zero+1; end end %k時(shí)刻的定位位置 if num_zero == N Xnode_g(k,i) = Xnode_g(k-1,i); Ynode_g(k,i) = Ynode_g(k-1,i); end

46、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 %粒子群補(bǔ)全N個(gè) if num_zero == N for j = 1:N lx(i,j,k) = 0;

47、 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(i,j,k) = Ynode_g(k,i); end

48、 end end end %第三種情況 的 上一時(shí)間沒有錨節(jié)點(diǎn)，也就是沒有采樣粒子的更新，這種情況下采樣盒子就是錨盒子 end while(sum(and)~=0 && ns ~= ns_range) %從采樣盒子里采樣 lx(i,j,k) = unifrnd(Xsambox_min(i,j,

49、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:temp2 d1(m) = sqrt((lx(i,j,k)-Xseed(k,A2(m)))^2+(ly(i,j,k)-Yseed(k,A2(m)))^

50、2); if d1(m)>tr && d1(m)<=2*tr and = [and 0]; else and = [and 1]; end end ns = ns+1; end %進(jìn)

51、行最終的判斷 and = []; 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

52、and = [and 1]; end end if sum(and)~=0 lx(i,j,k) = 0; ly(i,j,k) = 0; end end %至此 節(jié)點(diǎn) i 的粒子群已經(jīng)得到 %下面是進(jìn)行定位，粒子群的優(yōu)化

53、 num_zero = 0; for j = 1:N if lx(i,j,k) == 0 num_zero = num_zero+1; end end %k時(shí)刻的定位位置 if num_zero == N Xnode_g(k,i) = Xnode_g(k-1,i); Ynode_g(k,i) = Yno

54、de_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 %粒子群補(bǔ)全N個(gè) if num_zero == N for j = 1:N

55、 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(i,j,k) = Ynode_g(k,

56、i); end end end end %第四種情況 的 上一時(shí)間有錨節(jié)點(diǎn)，也就是有采樣粒子的更新 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*

57、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) Ynode_g(k-1,i)+Nf*Vmax]; Xsambox_min(i,j,k) = max(sambox1); Xsambox_max(i,j,k

58、) = min(sambox2); Ysambox_min(i,j,k) = max(sambox3); Ysambox_max(i,j,k) = min(sambox4); %增加一步判斷項(xiàng)，若是錨盒子和運(yùn)動(dòng)圓能夠有交集，就使用交集，如果沒有交集，則使用錨盒子 if Xsambox_min(i,j,k)>=Xsambox_max(i,j,k) || Ysambox_min(i,j,k)>=Ysambox_max(i,j,k)

59、 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 %從采樣盒子里采樣

60、 %從采樣盒子里采樣 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(

61、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 if d1(m)>tr and = [and 1]; end end

62、 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];

63、 end end ns = ns+1; end %進(jìn)行最終的判斷 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);

64、 if d1(m)<=tr and = [and 0]; end if d1(m)>tr and = [and 1]; end end for m = 1:temp2 d1(m) = sqrt((lx(i,j,k)-Xseed(k,A2(m)))^

65、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 if sum(and)~=0

66、 lx(i,j,k) = 0; ly(i,j,k) = 0; end end %至此 節(jié)點(diǎn) i 的粒子群已經(jīng)得到 %下面是進(jìn)行定位，粒子群的優(yōu)化 num_zero = 0; for j = 1:N if lx(i,j,k) == 0 num_zero = num_zero+1; end end %k時(shí)刻的定位位置 if num_zero == N Xnode_g(k,i) = Xnode_g(k-1,i); Ynode_g(k,i) = Ynode_g(k-1,i);