apda.m

多模型和概率数据关联结合后的IMMPDA算法

%PDA匀加速运动
function [x_fil,p_fil,likehood]=APDA(z_mes,x_mix_est,p_mix_est,clutter)
global C;
global T;
FV=[ 1, 0, T, 0, T*T/2, 0; 0, 1, 0, T, 0, T*T/2; 0, 0, 1, 0, T, 0; 0, 0, 0, 1, 0 ,T;0, 0, 0, 0, 1, 0;0, 0, 0, 0, 0, 1];%匀加速运动状态转移矩阵
GV=[T*T/2 0;0 T*T/2;T 0;0 T;1 0;0 1];
H=[1 0 0 0 0 0;0 1 0 0 0 0];           %匀速运动测量矩阵
Q=[10 0;0 10];

R=[100^2,0;0,100^2]; 
PG=0.99997;
PD=1;
z_all=clutter;
x_predict=FV*x_mix_est;
p_predict=FV*p_mix_est*FV'+GV*Q*GV';
z_predict=H*x_predict;
Sk=H*p_predict*H'+ R;
Kg=3.5;
d1=Kg*Sk(1,1);
d2=Kg*Sk(2,2);
% VG=(2*Kg)^2*sqrt(Sk(1,1))*sqrt(Sk(2,2));    %关联门
VG=(2*Kg)^2;
j=0;
z_valid=[];     %确认量测
z_error=[];     %残差
for i=1:C;
    if abs(z_all(1,i)-x_predict(1,1))<=d1;
        if abs(z_all(2,i)-x_predict(2,1))<=d2;
            z=z_all(:,i);
            z_valid=[z_valid z];
            temp=z-z_predict;
            z_error=[z_error temp];
            j=j+1;
        end
    end
end   
z_valid=[z_valid z_mes];
mk=j+1;
temp=z_mes-z_predict;
z_error=[z_error temp];
% mk=j;
% disp(mk);
lamda=0.0004;

if mk==0
    x_fil=x_predict;
    p_fil=p_predict;
else
    bk=lamda*(2*pi)*sqrt(det(Sk))*(1-PD*PG)/PD;
    
    e=[];
    for j=1:mk
        vi=z_error(:,j);
        e(j)=exp((-1/2)*vi'*inv(Sk)*vi);
    end
    %*******************************************
    beta_0=bk/(sum(e)+bk);
    beta_i=e./(sum(e)+bk);
    if beta_0 > 0.1
        beta_0=0.1;
        beta_i=0.9*e./sum(e);
    end
    
%     disp(beta_i);
    zkk1=beta_i.*z_error(1,:);
    zkk2=beta_i.*z_error(2,:);
    zkk=[sum(zkk1);sum(zkk2)];
    op=[0 0;0 0];
    for u=1:mk
        op1=z_error(:,u)*(z_error(:,u))';
        op=op1*beta_i(u)+op;
    end
    K=p_predict*H'*inv(Sk);
    x_fil=x_predict+K*zkk;
    PK=K*(op-zkk*zkk')*K';
    p_fil=beta_0*p_predict+(1-beta_0)*(eye(6)-K*H)*p_predict+PK;
% p_fil=p_predict-K*Sk*K';
    %计算似然函数
%     miufmk=[];
%     for i=-1:mk
%         miufmk(i+2)=exp(-lamda*VG)*(lamda*VG)^mk/factorial(mk); 
%     end 
%     gama0=(1-PD*PG)*miufmk(2)/miufmk(1)*(PD*PG+(1-PD*PG)*miufmk(2)/miufmk(1))^-1;
    gama0=(1-PD*PG)*lamda*VG/(PD*PG*mk+(1-PD*PG)*lamda*VG);
    for i=1:mk
        gama(i)=PD*PG/(PD*PG*mk+(1-PD*PG)*lamda*VG);
    end
    likehoodmea=[];
    for i=1:mk
        likehoodmea(i)=e(i)/(2*pi*sqrt(det(Sk)));
    end
    temp=[];
    for i=1:mk
        LL=likehoodmea(i)*gama(i)*PG^(-1);
        temp=[temp LL];
    end
    likehood=VG^(-mk)*gama0+VG^(-mk+1)*sum(temp);
    
end