%机动目标跟踪仿真,采用交互式多模(IMM)算法,使用三个模型
% function IMM(N)
tic
T=2;%雷达扫描周期
r=10000;%距离观测误差均方差
R=[r 0;0,r];
A1=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1];
H1=[1 0 0 0; 0 0 1 0];
G1=[T/2 0; 1 0; 0 T/2; 0 1];
A2=[1 T 0 0 T^2/2 0; 0 1 0 0 T 0
0 0 1 T 0 T^2/2; 0 0 0 1 0 T
0 0 0 0 1 0;0 0 0 0 0 1];
H2=[1 0 0 0 0 0; 0 0 1 0 0 0];
G2=[T^2/4 0; T^2/2 0
0 T^2/4; 0 T^2/2
1 0; 0 1];
q1=0;q2=0.05;q3=0.02;
Q1=[q1 0; 0 q1];
Q2=[q2 0; 0 q2];
Q3=[q3 0; 0 q3];
P=[0.95 0.025 0.025
0025 0.95 0.025
0.025 0.025 095]; %模型转移概率矩阵
U=[1 0 0]';
%模拟目标真实运动轨迹
x0=2000;y0=10000;v0=-15;
alx=0.075;aly=0.075;a2x=-0.3;a2y=-0.3;
t1=400;K1=t1/T;
t2=600;K2=(t2-t1)/T;
t3=610;K3=(t3-t2)/T;
t4=660;K4=(t4-t3)/T;
t5=1500;K5=(t5-t4)/T;
%0--400s,匀速运动
k1=1:K1;
x1=x0*ones(1,length(k1));
y1=y0+v0*k1*T;
%400--600s,慢转弯
k2=1:K2;
x2=x1(K1)+alx*(k2*T).^2/2;
y2=y1(K1)+v0*(k2*T)+aly*(k2*T).^2/2;
%600--660s,匀速运动
k3=1:K3;
x3=x2(K2)+alx*(t2-t1)*(k3*T);
y3=y2(K2)*ones(1,length(k3));
%610--660,快拐弯
k4=1:K4;
x4=x3(K3)+alx*(t2-t1)*k4*T+a2x*(k4*T).^2/2;
y4=y3(K3)+a2y*(k4*T).^2/2;
%660--1500s,匀速运动
k5=1:K5;
x5=x4(K4)*ones(1,length(k5));
y5=y4(K4)+a2y*(t4-t3)*(k5*T);
x=[x1,x2,x3,x4,x5];
y=[y1,y2,y3,y4,y5];
K=K1+K2+K3+K4+K5;
xx=zeros(1,K);yy=zeros(1,K);
ex1=zeros(1,K);ex2=zeros(1,K);ey1=zeros(1,K);ey2=zeros(1,K);
%进行N次仿真
N=50;
for i=1:N
zx=randn(1,K)*100+x;
zy=randn(1,K)*100+y;
Z=[zx;zy];%产生观测数据
LX=[zx(2) (zx(2)-zx(1))/T zy(2) (zy(2)-zy(1))/T]';%两点初始化状态向量
LP=[r r/T 0 0
r/T 2*r/(T*T) 0 0
0 0 r r/T
0 0 r/T 2*r/(T*T)];%两点初始化协方差矩阵
xx(1)=xx(1)+zx(1);yy(1)=yy(1)+zy(1);
ex1(1)=ex1(1)+x(1)-zx(1);ex2(1)=ex2(1)+(x(1)-zx(1))^2;
ey1(1)=ey1(1)+y(1)-zy(1);ey2(1)=ey2(1)+(y(1)-zy(1))^2;
%从第M+1次采样开始采用3模型的IMM算法
LX1=[LX',0,0]';
LX2=[LX',0,0]';
LX3=[LX',0,0]';
LP1=[LP,zeros(4,2);zeros(2,6)];
LP2=[LP,zeros(4,2);zeros(2,6)];
LP3=[LP,zeros(4,2);zeros(2,6)];
for k=2:K
xx(k)=xx(k)+LX(1);
yy(k)=yy(k)+LX(3);
ex1(k)=ex1(k)+x(k)-LX(1);
ex2(k)=ex2(k)+(x(k)-LX(1))^2;
ey1(k)=ey1(k)+y(k)-LX(3);
ey2(k)=ey2(k)+(y(k)-LX(3))^2;
%形成混合初始条件
C_=P'*U;
U=P.*(U*ones(1,3))./(ones(3,1)*C_');
X01=LX1*U(1,1)+LX2*U(2,1)+LX3*U(3,1);
X02=LX1*U(1,2)+LX2*U(2,2)+LX3*U(3,2);
X03=LX1*U(1,3)+LX2*U(2,3)+LX3*U(3,3)
P01=U(1,1)*(LP1+(LX1-X01)*(LX1-X01)')+...
U(2,1)*(LP2+(LX2-X01)*(LX2-X01)')+...
U(3,1)*(LP3+(LX3-X01)*(LX3-X01)');
P02=U(1,2)*(LP1+(LX1-X02)*(LX1-X02)')+...
U(2,2)*(LP2+(LX2-X02)*(LX2-X02)')+...
U(3,2)*(LP3+(LX3-X02)*(LX3-X02)');
P03=U(1,3)*(LP1+(LX1-X03)*(LX1-X03)')+...
U(2,3)*(LP2+(LX2-X03)*(LX2-X03)')+...
U(3,3)*(LP3+(LX3-X03)*(LX3-X03)');
%各模型滤波器滤波,计算似函数然
X01=X01(1:4);
P01=P01(1:4,1:4);
[LX1,LP1,E1]=kfilter(A1,H1,G1,Q1,R,X01,P01,Z,k);%------------------------------
LX1=[LX1',0,0]';
LP1=[LP1,zeros(4,2);zeros(2,6)];
[LX2,LP2,E2]=kfilter(A2,H2,G2,Q2,R,X02,P02,Z,k);%===============================
[LX3,LP3,E3]=kfilter(A2,H2,G2,Q3,R,X03,P03,Z,k);%0000000000000000000000000000000
E=[E1,E2,E3];
C=E*C_;
U=(E'.*C_)/C;
%模型概率修正
LX=LX1*U(1)+LX2*U(2)+LX3*U(3);
LP=U(1)*(LP1+(LX1-LX)*(LX1-LX)')+...
U(2)*(LP2+(LX2-LX)*(LX2-LX)')+...
U(3)*(LP3+(LX3-LX)*(LX3-LX)');
end
end
%计算滤波误差
for k=1:K
ex_(k)=ex1(k)/N;
ex(k)=sqrt(ex2(k)/N-ex_(k)^2);
ey_(k)=ey1(k)/N;
ey(k)=sqrt(ey2(k)/N-ey_(k)^2);
xx(k)=xx(k)/N;
yy(k)=yy(k)/N;
end
%输出图形
figure(1);
plot(x,y,'m',zx,zy,xx,yy,'k');
title('真实轨迹,测量误差及滤波估计');
xlabel('x');ylabel('y');
figure(2);
plot(ex_);
title('x方向误差均值');
figure(3);
plot(ey_);
title('y方向误差均值');
ylabel('ey');xlabel('k');
figure(4);
plot(ex);
title('x方向滤波误差');
ylabel('ex');xlabel('k');
figure(5);
plot(ey);
title('y方向滤波误差');
ylabel('ey');xlabel('k');
toc