《wsn定位蒙特卡洛方法mcl的matlab實現(xiàn)源碼》由會員分享，可在線閱讀，更多相關(guā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; %采樣盒子確定時，估計位置要擴(kuò)大圓面積
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估計位置,初始值設(shè)置為真實位置
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è)這個定位過程耗時非常短
%仿真步數(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逐個進(jìn)行定位
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
%接下來要獲取采樣盒子，每個粒子都要有一個采樣盒子，然后還要進(jìn)行采樣，每個采樣盒子的采樣都要有一個次數(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é)點，沒法進(jìn)行定位，則粒子群全部置零
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
%進(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
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 的粒子群已經(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時刻的定位位置
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、
%粒子群補(bǔ)全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
%進(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)>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 的粒子群已經(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時刻的定位位置
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
%粒子群補(bǔ)全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 的粒子群已經(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時刻的定位位置
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個
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');
wsn定位蒙特卡洛方法mcl的matlab實現(xiàn)源碼
