f_immjpda.m

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

%计算目标号为target_no的目标的immjpda滤波信息
function [x_jpda,p_jpda,x_immjpda,p_immjpda]=f_immjpda(state_dm,target_no,flag,prob,mes,mes_total,x1,p1,x2,p2)
%本程序获得特定目标的jpda和immjpda的滤波结果
%x_jpda:单模型jpda状态向量组合
%p_jpda:单模型jpda状态协方差组合
%x_immjpda:
%p_immjpda: 
%调用无量测时预报值
%if flag==1%新目标
  [model_number,x_model_predict,p_model_predict,x_imm_predict,p_imm_predict]=f_imm_nomes(flag,target_no,x1,p1,x2,p2);
  %else%老目标
 % [model_number,x_model_predict,p_model_predict,x_imm_predict,p_imm_predict]=f_imm_nomes(flag,target_no,x1,p1,x2,p2);
 %end
  %调用imm算法获得利用指定量测进行滤波的结果
for mes_no=1:mes_total
   mes_now=mes(:,mes_no);%得到当前量测
   [x(mes_no).single_model,p(mes_no).single_model,x(mes_no).imm,p(mes_no).imm]=f_imm(flag,mes_now,target_no,x1,p1,x2,p2);
end

prob0=1-sum(prob);%没有量测关联该目标的概率
%单模型目标状态jpda
x_jpda=[];
for m=1:model_number%利用到了模型数
    x(m).jpda=prob0*x_model_predict(:,m);
    for j=1:mes_total
       x(m).jpda=x(m).jpda+prob(j)*x(j).single_model(:,m);  
    end
    x_jpda=[x_jpda x(m).jpda];
end
%单模型目标状态协方差jpda
p_jpda=[];
for m=1:model_number
    p_now=p_model_predict(:,(m-1)*state_dm+1:m*state_dm);
    p(m).jpda=p_now+x_model_predict( :,m)*x_model_predict(:,m)'-x(m).jpda*x(m).jpda';    
    p(m).jpda=prob0*p(m).jpda;
    for j=1:mes_total
       temp1=p(mes_no).single_model(:,(m-1)*state_dm+1:m*state_dm);
       temp1=temp1+x(j).single_model(:,m)*x(j).single_model(:,m)'-x(m).jpda*x(m).jpda';
       p(m).jpda=p(m).jpda+prob(j)*temp1;      
    end
    p_jpda=[p_jpda p(m).jpda];
end

%多模型目标状态immjpda

x_immjpda=prob0*x_imm_predict;
for j=1:mes_total
    x_immjpda=x_immjpda+prob(j)*x(j).imm;
end
%多模型目标状态协方差immjpda
p_immjpda=prob0*(p_imm_predict+x_imm_predict*x_imm_predict'-x_immjpda*x_immjpda');
for j=1:mes_total
    p_immjpda=p_immjpda+prob(j)*(p(j).imm+x(j).imm*x(j).imm'-x_immjpda*x_immjpda');
end