wsn定位蒙特卡洛方法mcl的matlab

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) tr 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 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