WSN定位蒙特卡洛方法MCL的MATLAB實(shí)現(xiàn)源碼

1、精選優(yōu)質(zhì)文檔-傾情為你奉上clear;clc;%初始化工作Ns = 20;Nn = 200;Vmax = 20;Xrange = 200;Yrange = 200;tr = 50;step = 20;N = 20;Nf = 3; %采樣盒子確定時(shí)，估計(jì)位置要擴(kuò)大圓面積ns_range = 200; %每個(gè)采樣盒子的最大采樣次數(shù)for i = 1:Ns Xseed(1,i)=rand(1,1)*Xrange; Yseed(1,i)=rand(1,1)*Yrange;endfor i = 1:Nn Xnode(1,i)=rand(1,1)*Xrange; Ynode(1,i)=rand(1,1)*

2、Yrange; Xnode_g(1,i)=Xnode(1,i); %MCL估計(jì)位置,初始值設(shè)置為真實(shí)位置 Ynode_g(1,i)=Ynode(1,i);end%初始時(shí)刻的粒子群，for every nodefor i = 1:Nn for j = 1:N lx(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*);%節(jié)點(diǎn)們開始運(yùn)動，每次定位完成才開始下一次運(yùn)動，這里假設(shè)這個(gè)定位過程耗時(shí)非常短%仿真步數(shù)for k=2:step %新的時(shí)刻，節(jié)點(diǎn)

3、們先運(yùn)動一下,RWP模型 for i = 1:Ns r = rand(1,1)*Vmax; thita = rand(1,1)*2*pi; Xseed(k,i) = Xseed(k-1,i) + r*cos(thita); 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 %對每一個(gè)node逐個(gè)進(jìn)行定位 for i = 1:Nn %測試每個(gè)seed是否可以用來定位 A1 = ; %存儲1跳錨

4、節(jié)點(diǎn)序號 A2 = ; %存儲2跳錨節(jié)點(diǎn)序號 for j = 1:Ns d1 = sqrt(Xnode(k,i)-Xseed(k,j)2+(Ynode(k,i)-Yseed(k,j)2); if d1tr & d1=2*tr for m = 1:Nn if m = i dmi = 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); if dmi=tr & dms=tr A2 = A2 j; end end end end

5、 end %接下來要獲取采樣盒子，每個(gè)粒子都要有一個(gè)采樣盒子，然后還要進(jìn)行采樣，每個(gè)采樣盒子的采樣都要有一個(gè)次數(shù)限制，這里還是要分四種情況 temp1 = size(A1,2); temp2 = size(A2,2); %第一種情況 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,j,k) = 0; end end %第二種情況 if temp1=0 & temp2=0 f

6、or j = 1:N sambox1 = 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(

7、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(m) = sqrt(lx(i,j,k)-Xseed(k,A1(m)2+(ly(i,j,k)-Yseed(k,A1(m)2); end end ns = ns+1; end %進(jìn)行最終的判斷 and = ; for m = 1:temp1 d1(m) = sqrt(lx(i

8、,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 %至此 節(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); Yn

9、ode_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 %粒子群補(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 = N for j = 1:N if lx(i,j,k) = 0 lx(i,j,k) = Xnode_g(k,i); ly(i,j,k) = Yno

10、de_g(k,i); end end end end %第三種情況 if temp1=0 & temp2=0 for j = 1:N sambox1 = 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(sa

11、mbox3); Ysambox_max(i,j,k) = min(sambox4); and = and 0; else and = and 1; end end ns = ns+1; end %進(jìn)行最終的判斷 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 and = and 1; end end if sum(and)=0 lx(i,j,k) = 0; ly(i,j,k) =

12、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); end if num_zero = N Xnode_g(k,i) = sum(lx(i,:,k)/(N-num_zero); Ynode_g(k,i) = sum(ly(i,:,k)/(

13、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 = 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 %第四種情況 if temp1=0 & temp2=0 for j = 1:N sambox1 = Xnode_g(k-1,i)-Nf*Vmax; sambox2 = Xnode_g(k-1

14、,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,

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

16、m)=2*tr and = and 0; else 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)化 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); end if num_zero = N