www.pudn.com > IMM.rar > IMMT.m
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))