test.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%测角传感器似然函数
% Target=Target-Nod;
% [thr0 r0]=cart2pol(Target(1),Target(2));
% 
% Reference=Reference-Nod;
% Target=[Reference(1)+0.001*randn Reference(2)+0.001*randn];
ar=4;
ao=(3/180)*pi;
Target=[15 15]
[thr0 r0]=cart2pol(Target(1),Target(2));
Target=[Target(1)+0.001*randn Target(2)+0.001*randn]

for i=1:99
    for j=1:99
        %%%%%%%%%%%
        x=i/2;
        y=j/2;
        x=x-25;
        y=y-25;
        x=x+Target(1);
        y=y+Target(2);
        [thr r]=cart2pol( x , y );
        %%%%%%%%%%%%
        if( thr>=0 & thr0>=0  )
            Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(thr-thr0)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
        end
        %%%%%%%%%%%%
        if( thr<0 & thr0<0)
            Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(thr-thr0)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
        end
        %%%%%%%%%%%%
        if( thr>=0 & thr0<0 )
            z1=thr;
            z2=pi-thr;
            z3=pi+thr0;
            z4=-thr0;
            if( (z2+z3)<=pi )
                Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(z2+z3)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
            else
                Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(z1+z4)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
            end
        end
        %%%%%%%%%%%%%
        if( thr<0 & thr0>=0 )
            z1=thr0;
            z2=pi-thr0;
            z3=pi+thr;
            z4=-thr;
            if( (z2+z3)<=pi )
                Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(z2+z3)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
            else
                Pcorrect(i,j)=1/2/pi/ao*exp( -0.5*(z1+z4)^2/ao^2)*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
            end
        end    
        %%%%%%%%%%%%
    end
end
%%%%%%%%%%%%
%经过仔细分析,这部分是对的
Pcorrect=Pcorrect/sum(sum(Pcorrect));
figure,pcolor(Pcorrect);
colormap(gray(10))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ar=3;
r0=13;

for i=1:99
    for j=1:99
        %%%%%%%%%%%
        x=i/2;
        y=j/2;
        x=x-25;
        y=y-25;
        [thr r]=cart2pol( x , y );
        
        Pcorrect(i,j)=1/2/pi*exp(-0.5*(r-r0)^2/ar^2)/ar/r; 
        
    end
end
%%%%%%%%%%%%
%经过仔细分析,这部分是对的
Pcorrect(50,50)=0;
Pcorrect=Pcorrect/sum(sum(Pcorrect));
figure,pcolor(Pcorrect);
colormap(gray(10))