main.m

非线性跟踪中贝叶斯滤波算法

clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化
W=200;  %该区域的长度
L=200;  %该区域的宽度
M=36;   %该区域内节点数
Nod=NodGen(W,L,M,3);    %生成节点分布图
ar=3;   %测距方差
ao=(3/180)*pi;  %测角方差
T=50;   %总的仿真时间
V=5;    %目标运动速度,这在本仿真中为已知量
av=1;   %策动噪声方差




Target_Real{1}=[25 25];     %第一时刻目标参考位置
Target_Real{1}=[25 25]+av*[randn randn];    %第一时刻目标真实位置
for t=1:T
%     Target_Real{t+1}=Target_Real{t}+V^0.5*[1 1]+av*randn*[1 1];
    Target_Real{t+1}=Target_Real{t}+V/2^0.5*[1 1]+av*randn*[1 1];
end
%以下生成量测
for t=1:T
    for m=1:M
        r(m)=( ( Nod{m}(1) - Target_Real{t}(1) )^2 + ( Nod{m}(2) - Target_Real{t}(2) )^2 )^0.5;    
    end
    Dot(t)=find(r==min(r));
    x=Target_Real{t}(1)-Nod{Dot(t)}(1);
    y=Target_Real{t}(2)-Nod{Dot(t)}(2);
    [thr r]=cart2pol(x,y);
    thr=thr+ao*randn;
    r=r+ar*randn;
    [x y]=pol2cart(thr,r);
    Target_Z{t}=[x y]+[Nod{Dot(t)}(1) Nod{Dot(t)}(2)];
end
%生成参考坐标
for t=1:T
    Refer{t}=[round(Target_Real{t}(1)) round(Target_Real{t}(2))];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%进行第一时刻的初始化,生成第一时刻的分布
Pbelief{1}=fun1( Nod{Dot(1)} , Target_Z{1} , Refer{1} , ar , ao );
figure,pcolor(Pbelief{1});

%根据分布得到滤波结果估计
Target_Esti{1}=fun2( Pbelief{1} , Refer{1} );
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下生成状态转移概率矩阵
for i=1:99
    for j=1:99
        x=i/2;
        y=j/2;
%         P_P(i,j)=exp( -(x-25-V^0.5)^2/2/av^2 - (y-25-V^0.5)^2/2/av^2 );
        P_P(i,j)=exp( -(x-25-V/2^0.5)^2/2/av^2 - (y-25-V/2^0.5)^2/2/av^2 );
    end
end
P_P=P_P/sum(sum(P_P));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%以下为贝叶斯滤波过程
for t=2:T
    t
    %%%%%%%%%%%%%%%%%%
    %预测过程 包括卷积过程及概率矩阵中心变换
    Pbelief_P{t}=conv2( Pbelief{t-1} , P_P , 'same' );
    a=Refer{t}(1)-Refer{t-1}(1);    a=a*2;
    b=Refer{t}(2)-Refer{t-1}(2);    b=b*2;
    P=zeros(99);
    for i=1:99
        for j=1:99
            if( (i-a)>0 & (j-b)>0 )
                P(i-a,j-b)=Pbelief_P{t}(i,j);
            end
        end
    end
    Pbelief_P{t}=P;
    %%%%%%%%%%%%%%%%%%%
    %生成似然函数
    Pzx{t}=fun1( Nod{Dot(t)} , Target_Z{t} , Refer{t} , ar , ao );
    %%%%%%%%%%%%%%%%%%
    %更新过程
    Pbelief{t}=Pbelief_P{t}.*Pzx{t};
    Pbelief{t}=Pbelief{t}/sum(sum(Pbelief{t}));
    Target_Esti{t}=fun2( Pbelief{t} , Refer{t} );
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%图示仿真结果
for t=1:T
    x(t)=Target_Real{t}(1);
    y(t)=Target_Real{t}(2);    
    x1(t)=Target_Z{t}(1);
    y1(t)=Target_Z{t}(2);
    x2(t)=Target_Esti{t}(1);
    y2(t)=Target_Esti{t}(2);
end
figure,plot(x,y,x1,y1,x2,y2,x,y,'.',x1,y1,'.',x2,y2,'.')
legend('真实目标轨迹','观测轨迹','滤波后轨迹')
axis([0 W 0 L])

for t=1:T
    D1(t)=( (x(t)-x1(t))^2 + (y(t)-y1(t))^2 )^0.5;
    D2(t)=( (x(t)-x2(t))^2 + (y(t)-y2(t))^2 )^0.5;
end
figure,plot(1:T,D1,1:T,D2)
legend('观测误差','滤波后误差')
(sum(D1.^2/T))^0.5
(sum(D2.^2/T))^0.5
sum(D1)/T
sum(D2)/T