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:NsXseed(1,i)=rand(1,1)*Xrange;Yseed(1,i)=rand(1,1)*Yrange;endfor i = 1:NnXnode(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 nodefor i = 1:Nnfor j = 1:Nlx(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*');%节点们开始运动，每次定位完成才开始下一次运动，这里假设这个定位过程耗时非常短%仿真步数for k=2:step%新的时刻，节点们先运动一下,RWP模型for i = 1:Nsr = 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) < 0Xnode(k,i) = Xnode(k-1,i) + r*cos(thita);Ynode(k,i) = Ynode(k-1,i) - r*sin(thita);endend%对每一个node逐个进行定位for i = 1:Nn%测试每个seed是否可以用来定位A1 = []; %存储1跳锚节点序号A2 = []; %存储2跳锚节点序号for j = 1:Nsd1 = sqrt((Xnode(k,i)-Xseed(k,j))^2+(Ynode(k,i)-Yseed(k,j))^2);if d1<=trA1 = [A1 j];endif d1>tr && d1<=2*trfor m = 1:Nnif m ~= idmi = 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<=trA2 = [A2 j];endendendendend%接下来要获取采样盒子，每个粒子都要有一个采样盒子，然后还要进行采样，每个采样盒子的采样都要有一个次数限制，这里还是要分四种情况temp1 = size(A1,2);temp2 = size(A2,2);%第一种情况if temp1==0 && temp2==0Xnode_g(k,i)=Xnode_g(k-1,i);Ynode_g(k,i)=Ynode_g(k-1,i);for j = 1:Nlx(i,j,k) = 0; %周围没有锚节点，没法进行定位，则粒子群全部置零ly(i,j,k) = 0;endend%第二种情况if temp1~=0 && temp2==0for j = 1:Nsambox1 = [ 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:temp1d1(m) = sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);endendns = ns+1;end%进行最终的判断and = [];for m = 1:temp1d1(m) = sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);if d1(m)<=trand = [and 0];endif d1(m)>trand = [and 1];endendif sum(and)~=0lx(i,j,k) = 0;ly(i,j,k) = 0;endend%至此节点i 的粒子群已经得到%下面是进行定位，粒子群的优化num_zero = 0;for j = 1:Nif lx(i,j,k) == 0num_zero = num_zero+1;endend%k时刻的定位位置if num_zero == NXnode_g(k,i) = Xnode_g(k-1,i);Ynode_g(k,i) = Ynode_g(k-1,i);endif num_zero ~= NXnode_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 == Nfor j = 1:Nlx(i,j,k) = 0;ly(i,j,k) = 0;endendif num_zero ~= Nfor j = 1:Nif lx(i,j,k) == 0lx(i,j,k) = Xnode_g(k,i);ly(i,j,k) = Ynode_g(k,i);endendendend%第三种情况if temp1==0 && temp2~=0for j = 1:Nsambox1 = [ 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];elseand = [and 1];endendns = ns+1;end%进行最终的判断and = [];for m = 1:temp2d1(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*trand = [and 0];elseand = [and 1];endendif sum(and)~=0lx(i,j,k) = 0;ly(i,j,k) = 0;endend%至此节点i 的粒子群已经得到%下面是进行定位，粒子群的优化num_zero = 0;for j = 1:Nif lx(i,j,k) == 0num_zero = num_zero+1;endend%k时刻的定位位置if num_zero == NXnode_g(k,i) = Xnode_g(k-1,i);Ynode_g(k,i) = Ynode_g(k-1,i);endif num_zero ~= NXnode_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 == Nfor j = 1:Nlx(i,j,k) = 0;ly(i,j,k) = 0;endendif num_zero ~= Nfor j = 1:Nif lx(i,j,k) == 0lx(i,j,k) = Xnode_g(k,i);ly(i,j,k) = Ynode_g(k,i);endendendend%第四种情况if temp1~=0 && temp2~=0for j = 1:Nsambox1 = [ 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:temp1d1(m) = sqrt((lx(i,j,k)-Xseed(k,A1(m)))^2+(ly(i,j,k)-Yseed(k,A1(m)))^2);if d1(m)<=trand = [and 0];endif d1(m)<=trand = [and 0];endif d1(m)>trand = [and 1];endd1(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*trand = [and 0];elseand = [and 1];endendif sum(and)~=0lx(i,j,k) = 0;ly(i,j,k) = 0;endend%至此节点i 的粒子群已经得到%下面是进行定位，粒子群的优化num_zero = 0;for j = 1:Nif lx(i,j,k) == 0num_zero = num_zero+1;endend%k时刻的定位位置if num_zero == NXnode_g(k,i) = Xnode_g(k-1,i);Ynode_g(k,i) = Ynode_g(k-1,i);endif num_zero ~= NXnode_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 == Nfor j = 1:Nlx(i,j,k) = 0;ly(i,j,k) = 0;error_total(k) = sum(error(k,:))/Nn;endk = 1:size(error_total,2)plot(k,error_total,'b-o');。