immt.m

根据《多源信息融合》一书中的IMM多模型跟踪算法所写的仿真程序

clear all;
test_d=150;
measure=test(test_d);          %调用test()函数产生虚拟仿真参数，输入变量是测量标准差
for i=1:171
    test(:,:,i)=[measure(1,1,i),measure(1,2,i)];
end
pai=[0.85 0.15;0.15,0.85];  %定义一步转移概率矩阵
miu1_CV=0;            %匀速运动模型在初始时刻正确的概率
miu1_CA=1;            %匀加速运动模型在初始时刻正确概率
T=0.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_temp(:,:,1)=[0 0;0 0];                %miu 初始化
miu(:,:,1)=[miu1_CV miu1_CA];
x1_2(:,:,1)=[10 10 10 20 0 0];   x2_2(:,:,1)=[10 10 10 20 5 5];         %混合估计
x1_1(:,:,1)=[10 10 10 20 0 0];   x2_1(:,:,1)=[10 10 10 20 5 5];         %一步预测
X1(:,:,1)=[10 10 10 20  0  0];   X2(:,:,1)=[10 10 10 20 5 5];           %滤波更新
%P1(:,:,1)=[5 0 0 0 0 0;0 5 0 0 0 0;0 0 5 0 0 0;0 0 0 5 0 0;0 0 0 0 7 0;0 0 0 0 0 7];     P2(:,:,1)=[5 0 0 0 0 0;0 5 0 0 0 0;0 0 5 0 0 0;0 0 0 5 0 0;0 0 0 0 7 0;0 0 0 0 0 7];% 模型重初始化方差
P1(:,:,1)=[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(:,:,1)=[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_1(:,:,1)=[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_1(:,:,1)=[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];%混合估计协方差阵
FC1(:,:,1)=[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];   
FC2(:,:,1)=[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]; %滤波更新方差
z1_e(:,:,1)=[0 0];           z2_e(:,:,1)=[0 0];                 %测量残差
S1(:,:,1)=[test_d,0;0,test_d]; S2(:,:,1)=[test_d,0;0,test_d];  R=[test_d*test_d,0;0,test_d*test_d];%测量协方差阵
like1(1)=1;     like2(1)=0;    %似然函数
K1(:,:,1)=[0 0; 0 0;0 0; 0 0;0 0;0 0];        K2(:,:,1)=[0 0; 0 0;0 0; 0 0;0 0;0 0];               %滤波增益
cc=0;           %计算模型概率更新
for k=1:170
    %第一步 模型条件重初始化
    %1。首先计算混合概率
    %计算c
    c(1)=pai(1,1)*miu(1,1,k)+pai(2,1)*miu(1,2,k);
    c(2)=pai(1,2)*miu(1,1,k)+pai(2,2)*miu(1,2,k);
    %计算miu_temp
    miu_temp(1,1,k)=pai(1,1)*miu(1,1,k)/c(1);   
    miu_temp(1,2,k)=pai(1,2)*miu(1,1,k)/c(2);
    miu_temp(2,1,k)=pai(2,1)*miu(1,2,k)/c(1);
    miu_temp(2,2,k)=pai(2,2)*miu(1,2,k)/c(2);
    %2.进行混合估计
    %匀速运动模型
    x1_2(:,:,k)=X1(:,:,k)*miu_temp(1,1,k)+X2(:,:,k)*miu_temp(2,1,k);
    P1_1(:,:,k)=(FC1(:,:,k)+(x1_1(:,:,k)-x1_2(:,:,k))'*(x1_1(:,:,k)-x1_2(:,:,k)))*miu_temp(1,1,k)+(FC2(:,:,k)+(x2_1(:,:,k)-x1_2(:,:,k))'*(x2_1(:,:,k)-x1_2(:,:,k)))*miu_temp(2,1,k);
    %匀加速运动模型
    x2_2(:,:,k)=X1(:,:,k)*miu_temp(1,2,k)+X2(:,:,k)*miu_temp(2,2,k);
    P2_1(:,:,k)=(FC1(:,:,k)+(x1_1(:,:,k)-x2_2(:,:,k))'*(x1_1(:,:,k)-x2_2(:,:,k)))*miu_temp(1,2,k)+(FC2(:,:,k)+(x2_1(:,:,k)-x2_2(:,:,k))'*(x2_1(:,:,k)-x2_2(:,:,k)))*miu_temp(2,2,k);
    %----------------------------------------------------------------------
    %第二步 模型条件滤波
    %1. 状态预测，分别计算每个模型的
    %1.1 计算匀速匀速运动模型
    x1_1(:,:,k+1)=FV*x1_2(:,:,k)';%暂不考虑过程噪声
    P1(:,:,k+1)=FV*P1_1(:,:,k)*FV';
    %1.2 计算匀加速运动模型
    x2_1(:,:,k+1)=FA*x2_2(:,:,k)';
    P2(:,:,k+1)=FA*P2_1(:,:,k)*FA';
    %2 分别计算预测残差及协方差阵
    %2.1 计算匀速运动模型
    z1_e(:,:,k+1)=test(:,:,k+1)'-H*x1_1(:,:,k+1)';%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    S1(:,:,k+1)=H*P1(:,:,k+1)*H'+R;
    %2.2 计算匀加速运动模型
    z2_e(:,:,k+1)=test(:,:,k+1)'-H*x2_1(:,:,k+1)';
    S2(:,:,k+1)=H*P2(:,:,k+1)*H'+R;
    %2.3 计算模型匹配函数
    like1(k+1)=exp(-0.5*z1_e(:,:,k+1)*inv(S1(:,:,k+1))*z1_e(:,:,k+1)')/(sqrt(det(2*3.1415926*S1(:,:,k+1))));
    like2(k+1)=exp(-0.5*z2_e(:,:,k+1)*inv(S2(:,:,k+1))*z2_e(:,:,k+1)')/(sqrt(det(2*3.1415926*S2(:,:,k+1))));
    %2.4 滤波更新
    %2.4.1 计算匀速运动滤波更新
    K1(:,:,k+1)=P1(:,:,k+1)*H'*inv(S1(:,:,k+1));
    X1(:,:,k+1)=x1_1(:,:,k+1)+(K1(:,:,k+1)*z1_e(:,:,k+1)')';
    FC1(:,:,k+1)=P1(:,:,k+1)-K1(:,:,k+1)*S1(:,:,k+1)*K1(:,:,k+1)';
    %2.4.2 计算匀加速运动滤波更新
    K2(:,:,k+1)=P2(:,:,k+1)*H'*inv(S2(:,:,k+1));
    X2(:,:,k+1)=x2_1(:,:,k+1)+(K2(:,:,k+1)*z2_e(:,:,k+1)')';
    FC2(:,:,k+1)=P2(:,:,k+1)-K2(:,:,k+1)*S2(:,:,k+1)*K2(:,:,k+1)';
    %----------------------------------------------------------------------
    %第三步 模型概率更新
    c(1)=0;c(2)=0;
    for i=1:2
        for j=1:2
            c(i)=c(i)+pai(j,i)*miu(1,j,k);
        end
    end 
    cc=like1(k+1)*c(1)+like2(k+1)*c(2);
    miu(1,1,k+1)=like1(k+1)*c(1)/cc;
    miu(1,2,k+1)=like2(k+1)*c(2)/cc;
    %----------------------------------------------------------------------
    %第四步 估计融合
    X(:,:,k+1)=X1(:,:,k+1)*miu(1,1,k+1)+X2(:,:,k+1)*miu(1,2,k+1);
    P(:,:,k+1)=(FC1(:,:,k+1)+(X(:,:,k+1)-X1(:,:,k+1))*(X(:,:,k+1)-X1(:,:,k+1))')*miu(1,1,k+1)+(FC2(:,:,k+1)+(X(:,:,k+1)-X2(:,:,k+1))*(X(:,:,k+1)-X2(:,:,k+1))')*miu(1,2,k+1);
    %----------------------------------------------------------------------
end
for i=1:170;
%     x(i)=sqrt(X(1,1,i)*X(1,1,i)+X(1,2,i)*X(1,2,i));
%     ideal(i)=sqrt(measure(1,3,i)*measure(1,3,i)+measure(1,4,i)*measure(1,4,i));
%     noise(i)=sqrt(test(1,1,i)*test(1,1,i)+test(1,2,i)*test(1,2,i));
    x(i)=X1(1,1,i);
    ideal(i)=measure(1,3,i);
    noise(i)=test(1,1,i);
%     PP(i)=P(1,1,i);
end

% plot(i,)
% hold on
i=1:170;
plot(i,x(i),'r')
hold on
plot(i,noise(i),'g')
hold on
plot(i,ideal(i),'b')
hold on
% plot(i,PP(i))