shuipinjizhuanwan.m

％％目标在监控区内运动轨迹为水平转弯型曲线时的定位

function shuipinjizhuanwan
%%目标在监控区内运动轨迹为水平急转弯曲线时的定位%%

M=100;                                                                       %蒙特卡罗实验次数
N0=50;


%%初始化监控区域的长、宽、无线传感器个数和目标在监控区内的运动时间%%

a=100;%input('请输入监控区域的长\n');                                       %初始化监控区域的长
b=100;%input('请输入监控区域的宽\n');                                       %初始化监控区域的宽
s=100;%input('请输入无线传感器个数\n');                                     %初始化监控区域中的无线传感器个数
t=50;%input('请输入目标在监控区内的运动时间（单位：秒）\n');                  %选择观察某一监控时刻（单位：秒）
dn=0;%input('请输入联盟中损坏节点个数\n');

%初始化目标轨迹
n1=10;
n2=a-40;
n3=a-10;
dt=(a-20)/t;
for i=1:t
    if i*dt+n1<=n2
        Xt(i)=i*dt+n1;
        Yt(i)=b*0.8;
    elseif i*dt+n1<=n3&i*dt+n1>n2
        Xt(i)=i*dt+n1;
        Yt(i)=(0.7*b)*cos((Xt(i)-n2)*0.5*pi/(n2-n3))+0.1*b;
    end
end
Zt=zeros(1,t);                                                             %目标的垂直高度

%%初始化无线传感器的监控范围和传输范围%%

Ls=20;                                                                     %无线传感器的监测范围
Lt=20;                                                                     %无线传感器的传输范围
alfa=2;


%%初始化目标定位坐标的数组%%

Xr=zeros(1,t);                                                             %目标定位的横坐标
Yr=zeros(1,t);                                                             %目标定位的纵坐标
Xrd=zeros(1,t);                                                            %传感器受损情况下的目标定位的横坐标
Yrd=zeros(1,t);                                                            %传感器受损情况下的目标定位的纵坐标
Xm=zeros(1,t);                                                             %蒙特卡罗实验得到的横坐标
Ym=zeros(1,t);                                                             %蒙特卡罗实验得到的纵坐标
Xmd=zeros(1,t);                                                            %传感器受损情况下的蒙特卡罗实验得到的横坐标
Ymd=zeros(1,t);                                                            %传感器受损情况下的蒙特卡罗实验得到的纵坐标
D0=zeros(1,t);                                                             %距离误差
D0d=zeros(1,t);                                                            %距离误差
D1=zeros(1,t);                                                             %距离误差
D1d=zeros(1,t);                                                            %距离误差


%初始化一些传感器信息

p=0.0;                                                                     %无线传感器信息素在单位时间内的挥发后剩余量
de_mark=zeros(1,s);                                                        %传感器探测到目标的标志位


D=zeros(1,s);                                                              %初始化各传感器一个位置常量
E0=10000;                                                                  %传感器初始能量


for kk=1:N0
    Xm=zeros(1,t);
    Ym=zeros(1,t);
    Xmd=zeros(1,t);
    Ymd=zeros(1,t);
    for k=1:M
        X=rand(1,s)*a;                                                         %随机产生各传感器横坐标
        Y=rand(1,s)*b;                                                         %随机产生各传感器纵坐标
        neigh=zeros(s,s);                                                      %传感器的邻近节点表,储存节点序号
        [D,neigh]=dec(s,X,Y,D,Lt);                                             %计算位置常量
        w0=eps;                                                                %无线传感器的目标存在概率之和
        wd=eps;                                                                %无线传感器的目标存在概率之和
        N=zeros(1,s);                                                          %初始化各传感器的信息素值
        E=ones(1,s)*E0;                                                        %传感器能量
        for i=1:t
            N=p*N;                                                             %传感器信息素单位时间内的衰减
            w=zeros(1,s);                                                      %无线传感器在某一时刻的目标存在概率
            de_mark=zeros(1,s);                                                %传感器探测到目标的标志位
            dp=zeros(s,s);                                                     %传感器信息素增量表
            S=zeros(1,s);                                                      %初始化各传感器的工作状态。1代表活跃状态，0代表睡眠状态。
            Sd=zeros(1,s);                                                     %传感器受损情况下的各传感器的工作状态。
            for j=1:s                                                          %传感器信息素增量
                d=sqrt((X(j)-Xt(i))^2+(Y(j)-Yt(i))^2+Zt(i)^2)+eps;
                if d<=Ls
                    de_mark(j)=1;
                    N(j)=N(j)+(Ls/(d))^alfa;
                end
                if de_mark(j)==1                                               %如果传感器探测到目标则向外传播信息素
                    for m=1:s
                        if m~=j&neigh(j,m)==1
                            dp(j,m)=dp(j,m)+rat(j,m,X,Y,Lt,alfa)*N(j);
                            E(j)=E(j)-1;                                       %规定传感器广播一次信息素消耗1单位能量，其余行为不计消耗
                        end
                    end
                end
            end
            [S,Sd,N]=judg(N,de_mark,D,E,E0,s,Ls,alfa,dn,neigh,dp);
            for j=1:s %目标定位
                if S(j)==1
                    w(j)=2*atan(N(j))/pi;
                    Xr(i)=Xr(i)+X(j)*w(j);
                    Yr(i)=Yr(i)+Y(j)*w(j);
                    w0=w0+w(j);
                end
                if Sd(j)==1
                    w(j)=2*atan(N(j))/pi;
                    Xrd(i)=Xrd(i)+X(j)*w(j);
                    Yrd(i)=Yrd(i)+Y(j)*w(j);
                    wd=wd+w(j);
                end
            end
            Xr(i)=Xr(i)/w0;
            Yr(i)=Yr(i)/w0;
            w0=eps;
            Xrd(i)=Xrd(i)/wd;
            Yrd(i)=Yrd(i)/wd;
            wd=eps;
        end
        Xm=Xm+Xr;
        Ym=Ym+Yr;
        Xr=zeros(1,t);                                                         %目标定位的横坐标
        Yr=zeros(1,t);                                                         %目标定位的纵坐标
        Xmd=Xmd+Xrd;
        Ymd=Ymd+Yrd;
        Xrd=zeros(1,t);                                                        %目标定位的横坐标
        Yrd=zeros(1,t);                                                        %目标定位的纵坐标
    end
    Xm=Xm/M;
    Ym=Ym/M;
    Xmd=Xmd/M;
    Ymd=Ymd/M;
    D0=zeros(1,t);
    D0d=zeros(1,t);
    for i=1:t
        D0(i)=sqrt((Xt(i)-Xm(i))^2+(Yt(i)-Ym(i))^2);
        D0d(i)=sqrt((Xt(i)-Xmd(i))^2+(Yt(i)-Ymd(i))^2);
    end
    
    D1=D1+D0;
    D1d=D1d+D0;
end
D1=D1/N0;
D1d=D1d/N0;

figure
plot(Xt,Yt)
axis([0,a,0,b])
xlabel('单位：米')
ylabel('单位：米')
legend('目标轨迹')

figure
plot(Xt,Yt)
axis([0,a,0,b])
xlabel('单位：米')
ylabel('单位：米')
hold on
plot(Xm,Ym,'r')
legend('目标实际轨迹','定位轨迹')


figure
dmax=max(D1);
ddmax=max(D1d);
plot(D1,'-')
axis([1,t-1,0,ddmax*1.1])
xlabel('单位：秒')
ylabel('定位误差/米')
if dn>0
    hold on
    plot(D1d,'-.')
end
clear all

function r=rat(i,j,X,Y,Lt,alfa)
d=sqrt((X(i)-X(j))^2+(Y(i)-Y(j))^2);
r=(1-d/Lt)^alfa;

function [Q,neig]=dec(s,X,Y,D,Lt)
neig=zeros(s,s);
for i=1:s
    h0=0;
    k=0;
    h=zeros(1,s);
    for m=1:s
        d=sqrt((X(i)-X(m))^2+(Y(i)-Y(m))^2);
        if d<=2*Lt
           h(m)=d;
           k=k+1;
        end
        if d<=Lt
           neig(i,m)=1;
        end
    end
    for m=1:s
    h0=h0+h(m);
    end
    D(i)=h0/(2*(k-1)*Lt);
end
Q=D;

function [Q0,Qd,N1]=judg(N,de_mark,D,E,E0,s,Ls,alfa,dn,neigh,dp)
Q0=zeros(1,s);
Qd=zeros(1,s);
N1=zeros(1,s);
count=0;
for i=1:s
    if D(i)*N(i)*E(i)/E0>=(Ls/(Ls-2))^alfa&de_mark(i)==1
        Q0(i)=1;
        count=count+1;
    end
end
Qd=Q0;
if dn>0
    if count<dn
        Qd=zeros(1,s);
    else
        for i=1:dn
            for j=1:s
                if Qd(j)==1;
                    Qd(j)=0;
                    for m=1:s
                        if neigh(j,s)==1
                            Qd(m)=1;
                            N(m)=N(m)+dp(j,m);
                        end
                    end
                    break
                end
            end
        end
    end
end
N1=N;