f_imm.asv

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

function [x_single_model,p_single_model,x_imm,p_imm]=f_imm(flag,mes,target_no,x1,p1,x2,p2);
%说明，这里仅是个单模型卡尔曼滤波
model_try;
xforecast=fai*x1;
pforecast=fai*p1*fai'+g*q*g';
s=h*pforecast*h'+r;
km=pforecast*h'*inv(s);%5.k(k+1)//9*3
gm=mes-h*xforecast;%6.innovation
x_single_model=xforecast+km*gm;%7.xfilter
p_single_model=pforecast-km*s*km';%8.pfilter
x_imm=x_single_model;
p_imm=p_single_model;