f_confirm2.m

多目标跟踪 (三维)-算法matlab编制

function [confirm_mes,confirm_mes_index,confirm_mes_total,a,p,d_all]=f_confirm2(k,t,a0,p0,mes_total,measurement,r,yz2)
confirm_mes_total=0;
confirm_mes=[];
confirm_mes_index=[];
threshold=yz2;%根据开方分布得到的阈值，可调整
a=[];
p=[];
d_all=[];
kt=k*t;%当前采样时间
h=[1 kt kt^2/2 0 0 0 0 0 0;0 0 0 1 kt kt^2/2 0 0 0;0 0 0 0 0 0 1 kt kt^2/2];
z_predict=h*a0;
s=h*p0*h'+r;

for i=1:mes_total
    mes_now=measurement(:,i);
    %预报量测
    d=mes_now-z_predict;%新息
    d_statistic=d'*inv(s)*d;
    if d_statistic<threshold
       d_all=[d_all d_statistic];
       confirm_mes_total=confirm_mes_total+1;
       confirm_mes=[confirm_mes mes_now];
       confirm_mes_index=[confirm_mes_index i];
       [a_rt,p_rt]=f_poly_fit(k,t,mes_now,a0,p0,r);%多项式拟合
       a=[a a_rt];%相应于各个候选量测的多项式拟合系数（按列存放）
       p=[p p_rt]; 
    end
end