f_imm_predict1.m

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

function[z_predict,s_predict]=f_imm_predict1(i,x_jpda,p_jpda,u_input);
 %仅仿真使用，利用单模型代替多模型
 model_try;%给模型参数赋值
 predictProb=MarkoProb'*u_input;
 mixedInitX=zeros(9,model_number);
mixedInitP=zeros(9,9*model_number);
mixedInitC=zeros(9,9*model_number);
for i=1:model_number
    for j=1:model_number
        mixedProb(i,j)=MarkoProb(i,j)*u_input(i)/predictProb(j);
    end
end
for i=1:model_number
    for j=1:model_number
        mixedInitX(:,i)=mixedInitX(:,i)+x_jpda(:,j)*mixedProb(i,j);
    end
end
for i=1:model_number
    for j=1:model_number
        mixedInitC(:,(i-1)*9+1:i*9)=mixedInitC(:,(i-1)*9+1:i*9)+(x_jpda(:,j)-mixedInitX(:,i))*(x_jpda(:,j)-mixedInitX(:,i))'*mixedProb(i,j);
    end
end
for i=1:model_number
    for j=1:model_number
        mixedInitP(:,(i-1)*9+1:i*9)=mixedInitP(:,(i-1)*9+1:i*9)+p_jpda(:,(j-1)*9+1:j*9)*mixedProb(i,j);
    end
    mixedInitP(:,(i-1)*9+1:i*9)=mixedInitP(:,(i-1)*9+1:i*9)+mixedInitC(:,(i-1)*9+1:i*9);
end
 hh=[eye(9),eye(9),eye(9),eye(9),eye(9),eye(9)]';%计算I阵
 pp=[mixedInitP(:,1:9),zeros(9),zeros(9),zeros(9),zeros(9),zeros(9);zeros(9),mixedInitP(:,10:18),zeros(9),zeros(9),zeros(9),zeros(9);
    zeros(9),zeros(9),mixedInitP(:,19:27),zeros(9),zeros(9),zeros(9);zeros(9),zeros(9),zeros(9),mixedInitP(:,28:36),zeros(9),zeros(9);
zeros(9),zeros(9),zeros(9),zeros(9),mixedInitP(:,37:45),zeros(9); zeros(9),zeros(9),zeros(9),zeros(9),zeros(9),mixedInitP(:,46:54)];%计算P阵
xx=[mixedInitX(:,1)',mixedInitX(:,2)',mixedInitX(:,3)',mixedInitX(:,4)',mixedInitX(:,5)',mixedInitX(:,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;
 %x_predict=fai*x_immjpda;%x:两列（两个目标）四行（四维状态）xforecast是个列向量
 %z_predict=h*x_predict;%3.observation forecast
 %p_predict=fai*p_immjpda*fai'+g*q*g';%mbs个p矩阵的合并
 %s_predict=h*p_predict*h'+r;%新息协方差