f_extend.m

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

function [b1,cost,a,p]=f_extend(r,k,t,a0,p0,mes_index,mes_start,mes_total1,measurement,yz4);
h0=[1 k*t (k*t)^2/2 0 0 0 0 0 0;0 0 0 1 k*t (k*t)^2/2 0 0 0;0 0 0 0 0 0 1 k*t (k*t)^2/2];
h1=[1 (k+1)*t ((k+1)*t)^2/2 0 0 0 0 0 0;0 0 0 1 (k+1)*t ((k+1)*t)^2/2 0 0 0;0 0 0 0 0 0 1 (k+1)*t ((k+1)*t)^2/2];
s=h1*p0*h1'+r;

threshold=yz4;%根据开方分布得到的阈值，可调整
b1=[];%返回k+1时刻和k+2时刻的两个可行量测
a=[];
p=[];
cost=[];
z_predict=h1*a0;

for i=1:mes_total1
    mes_now=measurement(:,i);
    d=mes_now-z_predict;%新息
    d_statistic=d'*inv(s)*d;
    if d_statistic<threshold
       temp=[mes_index i];
       b1=[b1;temp];
       [a_rt,p_rt]=f_poly_fit(k+1,t,mes_now,a0,p0,r);%多项式拟合
       a=[a a_rt];
       p=[p p_rt];
       temp=(mes_start-h0*a_rt)'*inv(r)*(mes_start-h0*a_rt);
       temp=temp+(mes_now-h1*a_rt)'*inv(r)*(mes_now-h1*a_rt);
       cost=[cost temp];
    end
end