clear;
clc;
%初始化工作
Ns = 20;
Nn = 200;
Vmax = 20;
Xrange = 200;
Yrange = 200;
tr = 50;
step = 20;
N = 20;
Nf = 3; %采样盒子确定时，估计位置要扩大圆面积
ns_range = 200; %每个采样盒子的最大采样次数
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,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);
%plot(Xseed,Yseed,'bo',Xnode,Ynode,'k*');
%节点们开始运动，每次定位完成才开始下一次运动，这里假设这个定位过程耗时非常短%仿真步数
for k=2:step
%新的时刻，节点们先运动一下,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
%对每一个node逐个进行定位
for i = 1:Nn
%测试每个seed是否可以用来定位
A1 = []; %存储1跳锚节点序号
A2 = []; %存储2跳锚节点序号
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
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
end
%接下来要获取采样盒子，每个粒子都要有一个采样盒子，然后还要进行采样，每个采样盒子的采样都要有一个次数限制，这里还是要分四种情况
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; %周围没有锚节点，没法进行定位，则粒子群全部置零
ly(i,j,k) = 0;
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(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),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
%进行最终的判断
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
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_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(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,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);
and = [and 0];
else
and = [and 1];
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)>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) = 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_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(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,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,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 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(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
%至此节点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_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;
error_total(k) = sum(error(k,:))/Nn;
end
k = 1:size(error_total,2)
plot(k,error_total,'b-o');