fun2.m

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

function Target_Esti = fun2( Pbelief , Reference )
%根据概率分布矩阵求得对目标位置的估计
ux=0;
uy=0;
for i=1:99
    for j=1:99
        x=(i-50)*0.5;
        y=(j-50)*0.5;
        ux=ux+x*Pbelief(i,j);
        uy=uy+y*Pbelief(i,j);
    end
end
Target_Esti(1)=Reference(1)+ux;
Target_Esti(2)=Reference(2)+uy;