%計算目標號為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);
 

 
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)+(x1(:,j)-mixedInitX(:,i))*(x1(:,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(:,j)+k(:,(j-1)*3+1:j*3)*v(i).model_filter(:,j);%modified,j or i?
    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
    e=0;
    b=lmd*sqrt(det(2*3.14*s(:,(m-1)*3+1:m*3)))*(1-pd*pg)/pd;
    for i=1:mes_total
        
        e=e+exp(-0.5*v(i).model_filter(:,m)'*inv(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=zeros(9,1);
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=zeros(9);
for m=1:model_number
    c=c+(x(m).jpda-x_immjpda)*(x(m).jpda-x_immjpda)'*u_output(m);
end
p_immjpda=zeros(9);
for m=1:model_number
    p_immjpda=p_immjpda+p(m).jpda*u_output(m);
end
p_immjpda=p_immjpda+c;
 u_output=u_output';
%。。。。。。。。。。。。。。。。。。（模型綜合，非量測綜合）