《wsn定位蒙特卡洛方法mcl的matlab實現(xiàn)源碼》由會員分享，可在線閱讀，更多相關《wsn定位蒙特卡洛方法mcl的matlab實現(xiàn)源碼（11頁珍藏版）》請在裝配圖網(wǎng)上搜索。

1、 clear; clc; %初始化工作 Ns = 20; Nn = 200; Vmax = 20; Xrange = 200; Yrange = 200; tr = 50; step = 20; N = 20; Nf = 3; %采樣盒子確定時，估計位置要擴大圓面積 ns_range = 200; %每個采樣盒子的最大采樣次數(shù) for i = 1:Ns Xseed(1,i)=rand(1,1)*Xrange; Yseed(1,i)=rand(1,1)*Yrange; end for i = 1:Nn Xnode(1,

2、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 %初始時刻的粒子群，for every node for i = 1:Nn for j = 1:N lx(i,j,1) = Xnode_g(1,i); ly(i,j,1) = Ynode_g(1,i); end end %figure(1

3、); %plot(Xseed,Yseed,'bo',Xnode,Ynode,'k*'); %節(jié)點們開始運動，每次定位完成才開始下一次運動，這里假設這個定位過程耗時非常短 %仿真步數(shù) for k=2:step %新的時刻，節(jié)點們先運動一下,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

4、) > 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跳錨節(jié)點序號 A2 = []; %存儲2跳錨節(jié)點序號

5、 for j = 1:Ns d1 = sqrt((Xnode(k,i)-Xseed(k,j))^2+(Ynode(k,i)-Yseed(k,j))^2); if d1<=tr A1 = [A1 j]; end if d1>tr && d1<=2*tr for m = 1:Nn if m ~= i

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

7、 end end end end %接下來要獲取采樣盒子，每個粒子都要有一個采樣盒子，然后還要進行采樣，每個采樣盒子的采樣都要有一個次數(shù)限制，這里還是要分四種情況 temp1 = size(A1,2); temp2 = size(A2,2); %第一種情況 if temp1==0 && temp2==0

8、 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é)點，沒法進行定位，則粒子群全部置零 ly(i,j,k) = 0; end end %第二種情況 if temp1~=0 && tem

9、p2==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);

10、 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) = unifr

11、nd(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

12、 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 a

13、nd = [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

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

15、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))/(N-num_zero); end

16、 %粒子群補全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

17、 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 f

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

19、box_max(i,j,k) = min(sambox2); Ysambox_min(i,j,k) = max(sambox3); Ysambox_max(i,j,k) = min(sambox4); and = [and 0]; else and = [and 1];

20、 end end ns = ns+1; end %進行最終的判斷 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)>t

21、r && 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) = 0; end

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

23、 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))/(N-num_zero);

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

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

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

27、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),

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

29、 end 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,

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

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

32、 end end %k時刻的定位位置 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 %粒子群補全N個 if num_zero == N for j = 1:N lx(i,j,k) = 0; ly(i,j,k) = 0; error_total(k) = sum(error(k,:))/Nn; end k = 1:size(error_total,2) plot(k,error_total,'b-o');