function Pcorrect = fun1( Nod , Target , Reference , ar , ao )
%給定傳感器位置 實(shí)際的量測(cè)/目標(biāo)的位置 參考坐標(biāo)系 測(cè)距測(cè)角誤差
%其中Nod是(x,y)坐標(biāo)值,Target同樣也是(x,y)坐標(biāo)值
%求出的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
%%%%%%%%%%%%
%經(jīng)過仔細(xì)分析,這部分是對(duì)的
Pcorrect=Pcorrect/sum(sum(Pcorrect));