f_confirm1.m

kalman滤波程序

function [confirm_mes,confirm_mes_index,confirm_mes_total,a,p,d_all]=f_confirm1(k,t,mes_start,a0,p0,mes_total,measurement,vmin,vmax,r,yz1)
%为初始时刻各个候选航迹起点确定候选量测
confirm_mes_total=0;
confirm_mes=[];
confirm_mes_index=[];
threshold=yz1;%根据开方分布得到的阈值，可调整
a=[];
p=[];
d_all=[];

for i=1:mes_total
    mes_now=measurement(:,i);
    d=max(mes_now-mes_start-vmax*t,0)+max(-mes_now+mes_start+vmin*t,0);
    d_statistic=d'*inv(2*r)*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];
       vnow=(mes_now-mes_start)/t;
       [a_rt,p_rt]=f_poly_fit(k,t,mes_now,a0,p0,r);%多项式拟合
       a=[a a_rt];%相应于各个候选量测的多项式拟合系数（按列存放）
       p=[p p_rt]; 
    end
end