main.m

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

clc;
clear all;
global C;
global T;
C=10;
T=0.1;
test_d=100;
mm=test(test_d,T);          %调用test()函数产生虚拟仿真参数，输入变量是测量标准差
measure=[mm(1,:);mm(2,:)];
pai=[0.95 0.05;0.05,0.95];  %定义一步转移概率矩阵
miu1_CV=0;            %匀速运动模型在初始时刻正确的概率
miu1_CA=1;            %匀加速运动模型在初始时刻正确概率
FV=[ 1, 0, T, 0  0      0; 0, 1, 0, T  0      0; 0, 0, 1, 0  0  0; 0, 0, 0, 1  0  0;0  0  0  0  0  0;0  0  0  0  0  0];%匀速运动的状态转移矩阵
FA=[ 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];%匀加速运动状态转移矩阵
H=[1 0 0 0 0 0;0 1 0 0 0 0];           %匀速运动测量矩阵

miu=[miu1_CV miu1_CA];          %初始化miu
miu_mix=[0 0;0 0];              %初始化混合概率
x1_mix_est=[10 10 10 20 0 0]';   x2_mix_est=[10 10 10 20 5 5]';         %混合估计
x1_pre=[10 10 10 20 0 0]';   x2_pre=[10 10 10 20 30 5]';         %一步预测
x1_fil=[10 10 10 20 0 0]';   x2_fil=[10 10 10 20 30 5]';           %滤波更新
P1_mix_est=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 100 0 0 0;0 0 0 100 0 0;0 0 0 0 50 0;0 0 0 0 0 50];
P2_mix_est=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 100 0 0 0;0 0 0 100 0 0;0 0 0 0 50 0;0 0 0 0 0 50];  %混合估计协方差
P1_pre=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 350 0 0 0;0 0 0 350 0 0;0 0 0 0 370 0;0 0 0 0 0 370];
P2_pre=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 350 0 0 0;0 0 0 350 0 0;0 0 0 0 370 0;0 0 0 0 0 370];    %一步预测协方差
P1_fil=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 100 0 0 0;0 0 0 100 0 0;0 0 0 0 0 0;0 0 0 0 0 0];
P2_fil=[350 0 0 0 0 0;0 350 0 0 0 0;0 0 100 0 0 0;0 0 0 100 0 0;0 0 0 0 50 0;0 0 0 0 0 50];    %滤波协方差
z1_e=[0 0];           z2_e=[0 0];                 %测量残差
S1=[test_d,0;0,test_d]; S2=[test_d,0;0,test_d];   %
R=[test_d*test_d,0;0,test_d*test_d];              %测量协方差阵
like1=1;     like2=0;    %似然函数
K1=[0 0; 0 0;0 0; 0 0;0 0;0 0];        K2=[0 0; 0 0;0 0; 0 0;0 0;0 0];               %滤波增益
cc=0;           %计算模型概率更新



picXfil=[];
picX1fil=[];
picX2fil=[];
picPfil=[];
picP1fil=[];
picP2fil=[];
picmiu=[];

for k=1:200
    disp(k);
    %(1)相互作用
    %求miu_mix
    c(1)=pai(1,1)*miu(1,1)+pai(2,1)*miu(1,2);
    c(2)=pai(1,2)*miu(1,1)+pai(2,2)*miu(1,2);
    miu_mix(1,1)=pai(1,1)*miu(1,1)/c(1);
    miu_mix(1,2)=pai(1,2)*miu(1,1)/c(2);
    miu_mix(2,1)=pai(2,1)*miu(1,2)/c(1);
    miu_mix(2,2)=pai(2,2)*miu(1,2)/c(2);
    %求x_mix_est
    x1_mix_est=x1_fil*miu_mix(1,1)+x2_fil*miu_mix(2,1);
    x2_mix_est=x1_fil*miu_mix(1,2)+x2_fil*miu_mix(2,2);
    %求P_mix_est
    P1_mix_est=(P1_fil+(x1_fil-x1_mix_est)*(x1_fil-x1_mix_est)')*miu_mix(1,1)+(P2_fil+(x2_fil-x1_mix_est)*(x2_fil-x1_mix_est)')*miu_mix(2,1);
    P2_mix_est=(P1_fil+(x1_fil-x2_mix_est)*(x1_fil-x2_mix_est)')*miu_mix(1,2)+(P2_fil+(x2_fil-x2_mix_est)*(x2_fil-x2_mix_est)')*miu_mix(2,2);
    %(2)滤波
	z_mea=measure(:,k);
    [clutter]=createclutter(z_mea,C);
    [x1_fil,P1_fil,like1]=VPDA(z_mea,x1_mix_est,P1_mix_est,clutter);
    [x2_fil,P2_fil,like2]=APDA(z_mea,x2_mix_est,P2_mix_est,clutter);
    %(3)模型概率更新
    cc=like1*c(1)+like2*c(2);
    miu(1,1)=like1*c(1)/cc;
    miu(1,2)=like2*c(2)/cc;
    %(4)滤波综合
    X=x1_fil*miu(1,1)+x2_fil*miu(1,2);
    P=(P1_fil+(X-x1_fil)*(X-x1_fil)')*miu(1,1)+(P2_fil+(X-x2_fil)*(X-x2_fil)')*miu(1,2);
    picXfil=[picXfil X];
    picX1fil=[picX1fil x1_fil];
    picX2fil=[picX2fil x2_fil];
    picPfil=[picPfil P];
    picP1fil=[picP1fil P1_fil];
    picP2fil=[picP2fil P2_fil];
    picmiu=[picmiu miu'];
end   
i=1:200;
plot(i,picXfil(1,:),'r',i,measure(1,:),'g',i,mm(3,:),'b');
figure,plot(i,picX1fil(1,:),'c',i,measure(1,:),'g',i,mm(3,:),'b');
figure,plot(i,picX2fil(1,:),'y',i,measure(1,:),'g',i,mm(3,:),'b');

%plot(i,picXfil(3,:));