fun1.m

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

function Pcorrect = fun1( Nod , Target , Reference , ar , ao )
%给定传感器位置 实际的量测/目标的位置 参考坐标系 测距测角误差
%其中Nod是(x,y)坐标值,Target同样也是(x,y)坐标值
%求出的Belief是99*99的矩阵

Target=Target-Nod;
[thr0 r0]=cart2pol(Target(1),Target(2));

Reference=Reference-Nod;
Target=[Reference(1)+0.001*randn Reference(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));