f_initiate_predict.asv

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

function [z_rt, s_rt]=f_initiate_predict(x_current,p_current);
%新航迹下一时刻的预测
model_try;%给模型参数赋值
 hh=[eye(9),eye(9),eye(9),eye(9),eye(9),eye(9)]';%计算I阵
 pp=[p_current(:,1:9),zeros(9),zeros(9),zeros(9),zeros(9),zeros(9);zeros(9),p_current(:,10:18),zeros(9),zeros(9),zeros(9),zeros(9);
    zeros(9),zeros(9),p_current(:,19:27),zeros(9),zeros(9),zeros(9);zeros(9),zeros(9),zeros(9),p_current(:,28:36),zeros(9),zeros(9);
zeros(9),zeros(9),zeros(9),zeros(9),p_current(:,37:45),zeros(9); zeros(9),zeros(9),zeros(9),zeros(9),zeros(9),p_current(:,46:54)];%计算P阵
xx=[x_jpda(:,1)',x_jpda(:,2)',x_jpda(:,3)',x_jpda(:,4)',x_jpda(:,5)',x_jpda(:,6)'];
p_predict=inv(hh'*inv(pp)*hh);%计算多模型的预报状态
 x_predict=p_predict*hh'*inv(pp)*xx';
 z_predict=h*x_predict;
 s_predict=h*p_predict*h'+r; 
%model_try;%给模型参数赋值
 %x_predict=fai*x_current;%x:两列（两个目标）四行（四维状态）xforecast是个列向量
 %z_rt=h*x_predict;%3.observation forecast
 %p_predict=fai*p_current*fai'+g*q*g';%mbs个p矩阵的合并
 %s_rt=h*p_predict*h'+r;%新息协方差