www.pudn.com > Target_Tracking_Simulation_Program.rar > immkf.m, change:2005-06-25,size:3378b
function [XXE,YYE]=immkf(XX,YY,T,D)
%概率转移矩阵
P=[0.95 0.025 0.025
0.025 0.95 0.025
0.025 0.025 0.95];
%模型1参数(非机动)
q1=0;
Fai1=[1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1];
G1=[T/2 0
1 0
0 T/2
0 1];
H1=[1 0 0 0
0 0 1 0];
%模型2参数(机动)
q2=0.001;
Fai2=[1 T 0 0 T*T/2 0
0 1 0 0 T 0
0 0 1 T 0 T*T/2
0 0 0 1 0 T
0 0 0 0 1 0
0 0 0 0 0 1];
G2=[T*T/4 0
T/2 0
0 T*T/4
0 T/2
1 0
0 1];
H2=[1 0 0 0 0 0
0 0 1 0 0 0];
%模型3参数(机动)
q3=0.0114;
Fai3=Fai2;
G3=G2;
H3=H2;
%初始化
Z(1,:)=XX;
Z(2,:)=YY;
XE1=[XX(3),(XX(3)-XX(2))/T,YY(3),(YY(3)-YY(2))/T]';
PE1=[D D/T 0 0
D/T q1/4+2*D/(T*T) 0 0
0 0 D D/T
0 0 D/T q1/4+2*D/(T*T)];
XE2=[XX(3),(XX(3)-XX(2))/T,YY(3),(YY(3)-YY(2))/T,(XX(3)+XX(1)-2*XX(2))/T*T,(YY(3)+YY(1)-2*YY(2))/T*T]';
PE2=[D D/T 0 0 0 0
D/T q2*T*T/16+2*D/(T*T) 0 0 q2*T/4 0
0 0 D D/T 0 0
0 0 D/T q2*T*T/16+2*D/(T*T) 0 q2*T/4
0 q2*T/4 0 0 2*q2 0
0 0 0 q2*T/4 0 2*q2];
XE3=XE2;
PE3=[D D/T 0 0 0 0
D/T q3*T*T/16+2*D/(T*T) 0 0 q3*T/4 0
0 0 D D/T 0 0
0 0 D/T q3*T*T/16+2*D/(T*T) 0 q3*T/4
0 q3*T/4 0 0 2*q3 0
0 0 0 q3*T/4 0 2*q3];
U=[1 0 0];
N=length(XX);
%循环
for k=3:N
%输入交互
for j=1:3
c(j)=dot(P(:,j),U);
end
for i=1:3
for j=1:3
UU(i,j)=P(i,j)*U(i)/c(j);
end
end
X01=(XE1(1:4))*UU(1,1)+(XE2(1:4))*UU(2,1)+(XE3(1:4))*UU(3,1);
XE1(5)=0;XE1(6)=0;
X02=XE1*UU(1,2)+XE2*UU(2,2)+XE3*UU(3,2);
X03=XE1*UU(1,3)+XE2*UU(2,3)+XE3*UU(3,3);
P01=(PE1+(XE1(1:4)-X01)*(XE1(1:4)-X01)')*UU(1,1)+(PE2(1:4,1:4)+(XE2(1:4)-X01)*(XE2(1:4)-X01)')*UU(2,1)...
+(PE3(1:4,1:4)+(XE3(1:4)-X01)*(XE3(1:4)-X01)')*UU(3,1);
for i=5:6
for j=1:6
PE1(i,j)=0;
end
end
for i=5:6
for j=1:6
PE1(j,i)=0;
end
end
P02=(PE1+(XE1-X02)*(XE1-X02)')*UU(1,2)+(PE2+(XE2-X02)*(XE2-X02)')*UU(2,2)...
+(PE3+(XE3-X02)*(XE3-X02)')*UU(3,2);
P03=(PE1+(XE1-X03)*(XE1-X03)')*UU(1,3)+(PE2+(XE2-X03)*(XE2-X03)')*UU(2,3)...
+(PE3+(XE3-X03)*(XE3-X03)')*UU(3,3);
%模型条件滤波
%模型1
%X01=X01(1:4);
XP1=Fai1*X01;
PP1=Fai1*P01*Fai1';
K1=PP1*H1'*inv(H1*PP1*H1'+D*eye(2));
XE1=XP1+(K1*(Z(:,k)-H1*(XP1)));
PE1=(eye(4)-K1*H1)*PP1;
miu1=Z(:,k)-H1*(XP1);
S1=H1*PP1*H1'+D*eye(2);
A(1)=exp(-miu1'*inv(S1)*miu1/2)/((2*pi)*sqrt(det(S1)));
%模型2
XP2=Fai2*X02;
PP2=Fai2*P02*Fai2'+G2*q2*eye(2)*G2';
K2=PP2*H2'*inv(H2*PP2*H2'+D*eye(2));
XE2=XP2+K2*(Z(:,k)-H2*XP2);
PE2=(eye(6)-K2*H2)*PP2;
miu2=Z(:,k)-H2*XP2;
S2=H2*PP2*H2'+D*eye(2);
A(2)=exp(-miu2'*inv(S2)*miu2/2)/((2*pi)*sqrt(det(S2)));
%模型3
XP3=Fai3*X03;
PP3=Fai3*P03*Fai3'+G3*q3*eye(2)*G3';
K3=PP3*H3'*inv(H3*PP3*H3'+D*eye(2));
XE3=XP3+K3*(Z(:,k)-H3*XP3);
PE3=(eye(6)-K3*H3)*PP3;
miu3=Z(:,k)-H3*XP3;
S3=H3*PP3*H3'+D*eye(2);
A(3)=exp(-miu3'*inv(S3)*miu3/2)/((2*pi)*sqrt(det(S3)));
%求归一化常数
C=dot(A,c);
%求模型概率
for i=1:3
U(i)=A(i)*c(i)/C;
end
%输出交互
XXE(k)=XE1(1)*U(1)+XE2(1)*U(2)+XE3(1)*U(3);
YYE(k)=XE1(3)*U(1)+XE2(3)*U(2)+XE3(3)*U(3);
end%循环结束