f_immjpda_new.asv

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

%计算目标号为target_no的目标的immjpda滤波信息
%2004.1.9更新
function [x_jpda,p_jpda,x_immjpda,p_immjpda,u_output]=f_immjpda_new(state_dm,target_no,flag,prob,mes,mes_total,x1,p1,u_input,vv,pd,pg,lmd)
%v:跟踪门体积（需计算-长方体） 
%本程序获得特定目标的jpda和immjpda的滤波结果
%x_jpda:单模型多量测jpda状态向量组合
%p_jpda:单模型多量测jpda状态协方差组合
%x_immjpda:多模型多量测状态向量
%p_immjpda:多模型多量测状态协方差
%u_input:当前时刻模型的概率,为一列向量
%u_output:下一时刻模型的概率,为一列向量
%调用无量测时预报值

%调用imm算法获得利用指定量测进行滤波的结果
model_number=6;
%模型参数按列存放,各个模型的fai(j)是9×9矩阵,g(j)是9×3矩阵,q(j)是3×3矩阵
%h和r两个参数和模型无关
fai=[eye(9) eye(9) eye(9) eye(9) eye(9) eye(9)];
g=[eye(3) eye(3) eye(3) eye(3) eye(3) eye(3)
       zeros(6,3) zeros(6,3) zeros(6,3) zeros(6,3) zeros(6,3) zeros(6,3)];
q=[eye(3) eye(3) eye(3) eye(3) eye(3) eye(3)];
h=[eye(3) zeros(3,6)];
r=eye(3);
 

MarkoProb=zeros(model_number)+(1/6);
predictProb=MarkoProb'*u_input;
mixedProb=[];
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)+x1(:,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(:,j)-mixedInitX(:,i))*(x(:,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)+p1(:,(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

for i=1:model_number
    temp1=(i-1)*9+1;
    temp2=i*9;
    x_model_predict(:,i)=fai(:,temp1:temp2)*mixedInitX(:,i);
    p_model_predict(:,temp1:temp2)=fai(:,temp1:temp2)*mixedInitP(:,temp1:temp2)*fai(:,temp1:temp2)'+g(:,(i-1)*3+1:i*3)*q(:,(i-1)*3+1:i*3)*g(:,(i-1)*3+1:i*3)';
    s(:,(i-1)*3+1:i*3)=h*p_model_predict(:,temp1:temp2)*h'+r;
    k(:,(i-1)*3+1:i*3)=p_model_predict(:,temp1:temp2)*h'*inv(s(:,(i-1)*3+1:i*3));
    p_model_filter(:,temp1:temp2)=(eye(9)-k(:,(i-1)*3+1:i*3)*h)*p_model_predict(:,temp1:temp2);
end

for i=1:mes_total
    for j=1:model_number
        v(i).model_filter(:,j)=mes(:,i)-h*x_model_predict(:,j);
        x(i).model_filter(:,j)=x_model_predict(:,i)+k(:,(i-1)*3+1:i*3)*v(i).model_filter(:,j);
    end
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).model_filter(:,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)*9+1:m*9);
    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_model_filter(:,(m-1)*9+1:m*9);
       temp1=temp1+x(j).model_filter(:,m)*x(j).model_filter(:,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
c=0;
 
for m=1:model_number
    b=lmd*det(2*3.14*s(:,(m-1)*3+1:m*3))*(1-pd*pg)/pd;
    e=0;
    for i=1:mes_total
        e=e+exp(-0.5*v(i).model_filter(:,m)'*s(:,(m-1)*3+1:m*3)'*v(i).model_filter(:,m));
    end
    sanjiao(m)=(b+e)*pd*pg*vv^(1-mes_total)/mes_total;
    c=c+sanjiao(m)*predictProb(m);
end
x_immjpda=0;
for m=1:model_number
    u_output(m)=sanjiao(m)*predictProb(m)/c;
    x_immjpda=x_immjpda+x(m).jpda*u_output(m);
end
c=0;
for m=1:model_number
    c=c+(x(m).jpda-x_immjpda)*(x(m).jpda-x_immjpda)'*u_output(m);
end
p_immjpda=0;
for m=1:model_number
    p_immjpda=p_immjpda+p(m).jpda*u_output(m);
end
p_immjpda=p_immjpda+c;
%。。。。。。。。。。。。。。。。。。（模型综合，非量测综合）