1 简介
雷达与红外数据融合能够实现信息互补,改善对目标的跟踪,识别以 及提高系统的生存能力.为了解决空中目标高速机动时,单一模型的雷达/红外序贯滤波跟踪发散的问题,采用一种基于拓展卡尔马驴型的雷达/红外融合 跟踪机动目标的方法.仿真结果表明,该方法是一种雷达与红外传感器融合跟踪机动目标的有效方法.
2 部分代码
clear all;
close all;
%参数初始化
MCruns=50;%MC仿真次数
T=0.1;%采样周期
%状态转移矩阵
F=[1 T T^2/2 0 0 0 0 0 0;0 1 T 0 0 0 0 0 0;0 0 1 0 0 0 0 0 0;0 0 0 1 T T^2/2 0 0 0;0 0 0 0 1 T 0 0 0;0 0 0 0 0 1 0 0 0;0 0 0 0 0 0 1 T T^2/2;0 0 0 0 0 0 0 1 T;0 0 0 0 0 0 0 0 1];
%过程噪声增益矩阵
G=[T^3/6 0 0;T^2/2 0 0;T 0 0;0 T^3/6 0;0 T^2/2 0;0 T 0;0 0 T^3/6;0 0 T^2/2;0 0 T];
q1=10;%过程噪声方差
q2=15;
Q1=eye(3)*q1;
Q2=eye(3)*q2;
%状态向量初值
X1(1,:)=[8000,200,5,5000,400,1,9000,250,10];%目标1
X2(1,:)=[100,120,0.5,-6000,-300,0.5,5000,-100,15];%目标2
N=500;%仿真中采样长度
%用轨迹产生子函数模拟两个目标的真实轨迹
traj1=gen3trj(F,G,q1,X1,T,N);
traj2=gen3trj(F,G,q2,X2,T,N);
%雷达与红外传感器的量测误差设定
IR=[0.0001 0;0 0001];%IR为0.0001rad
R=[0.01 0 0;0 0.01 0;0 0 100];%radar为角度误差0.01rad，距离误差100m
for i=1:MCruns
%调用融合函数，计算跟踪轨迹
%目标1
[tmeas1,measIR1,measR1]=gen_meas(IR,R,traj1,T,i*(i+10));
Xt1=traj1(1,:);Xg1=0.9*Xt1';P1=diag((Xt1'-Xg1).^2);%设定初始值，此处初始x初始估计值设为0.9*Xt
[X1mf,P1mf,standard1varmf]=mf(F,G,Q1,IR,R,Xg1,P1,measIR1,measR1,traj1);%量测融合，得到状态向量，标准差
[X1svf,P1svf,standard1varsvf]=svf(F,G,Q1,IR,R,Xg1,P1,measIR1,measR1,traj1);%状态向量融合.....
%目标2
[tmeas2,measIR2,measR2]=gen_meas(IR,R,traj2,T,i*(i+10));
Xt2=traj2(1,:);Xg2=0.9*Xt2';P2=diag((Xt2'-Xg2).^2);
[X2mf,P2mf,standard2varmf]=mf(F,G,Q1,IR,R,Xg2,P2,measIR2,measR2,traj2);
[X2svf,P2svf,standard2varsvf]=svf(F,G,Q1,IR,R,Xg2,P2,measIR2,measR2,traj2);
%误差计算，为绘图做准备
%量测融合/状态向量融合“位置误差绝对值=实际值-估计值”
aberror1mf_x(i,:)=abs(traj1(:,1)-X1mf(:,1));aberror1mf_y(i,:)=abs(traj1(:,4)-X1mf(:,4));aberror1mf_z(i,:)=abs(traj1(:,7)-X1mf(:,7));
aberror1svf_x(i,:)=abs(traj1(:,1)-X1svf(:,1));aberror1svf_y(i,:)=abs(traj1(:,4)-X1svf(:,4));aberror1svf_z(i,:)=abs(traj1(:,7)-X1svf(:,7));
aberror2mf_x(i,:)=abs(traj2(:,1)-X2mf(:,1));aberror2mf_y(i,:)=abs(traj2(:,4)-X2mf(:,4));aberror2mf_z(i,:)=abs(traj2(:,7)-X2mf(:,7));
aberror2svf_x(i,:)=abs(traj2(:,1)-X2svf(:,1));aberror2svf_y(i,:)=abs(traj2(:,4)-X2svf(:,4));aberror2svf_z(i,:)=abs(traj2(:,7)-X2svf(:,7));
%融合位置标准差
standard1varsvf.P(i,:)=standard1varsvf.P;
standard2varsvf.P(i,:)=standard2varsvf.P;
standard1varmf.P(i,:)=standard1varmf.P;
standard2varmf.P(i,:)=standard2varmf.P;
%位置误差的方差和
Varmf1(i,:)=aberror1mf_x(i,:).^2+aberror1mf_y(i,:).^2+aberror1mf_z(i,:).^2;
Varmf2(i,:)=aberror2mf_x(i,:).^2+aberror2mf_y(i,:).^2+aberror2mf_z(i,:).^2;
Varsvf1(i,:)=aberror1svf_x(i,:).^2+aberror1svf_y(i,:).^2+aberror1svf_z(i,:).^2;
Varsvf2(i,:)=aberror2svf_x(i,:).^2+aberror2svf_y(i,:).^2+aberror2svf_z(i,:).^2;
end
% 结果分析与绘图
% 图1：通过量测融合进行轨迹显示
figure(1);
subplot(211);plot3(traj1(:,1),traj1(:,4),traj1(:,7),'b',X1mf(:,1),X1mf(:,4),X1mf(:,7),'r',traj2(:,1),traj2(:,4),traj2(:,7),'y',X2mf(:,1),X2mf(:,4),X2mf(:,7),'g');
grid on
xlabel('x');ylabel('y');zlabel('z')
title('量测融合-两个目标的模拟真实轨迹与跟踪')
subplot(212);plot3(traj1(:,1),traj1(:,4),traj1(:,7),'b',X1svf(:,1),X1svf(:,4),X1svf(:,7),'r',traj2(:,1),traj2(:,4),traj2(:,7),'y',X2svf(:,1),X2svf(:,4),X2svf(:,7),'g');
grid on
xlabel('x');ylabel('y');zlabel('z')
title('状态向量融合-两个目标的模拟真实轨迹与跟踪')
%图2，3：传感器各个量测值与真值对比图
t=(0:N)*T;%绘图时长T,1T,2T........NT
%目标1
figure(2)
subplot(321);plot(t(1:N),measIR1(1:N,1),'b');hold; plot(t(1:N),tmeas1(1:N,1),'r');ylabel('方位角/rad');title('目标1量测与真值对比')
subplot(323);plot(t(1:N),measIR1(1:N,2),'b');hold; plot(t(1:N),tmeas1(1:N,2),'r');ylabel('俯仰角/rad');
subplot(322);plot(t(1:N),measR1(1:N,1),'b');hold; plot(t(1:N),tmeas1(1:N,1),'r');ylabel('方位角/rad');
subplot(324);plot(t(1:N),measR1(1:N,2),'b');hold; plot(t(1:N),tmeas1(1:N,2),'r'); ylabel('俯仰角/rad');
subplot(326);plot(t(1:N),measR1(1:N,3),'b');hold; plot(t(1:N),tmeas1(1:N,3),'r');ylabel('径向距离/m');
%目标2
figure(3)
subplot(321);plot(t(1:N),measIR2(1:N,1),'y');hold; plot(t(1:N),tmeas2(1:N,1),'g');ylabel('方位角/rad');title('目标2量测与真值对比')
subplot(323);plot(t(1:N),measIR2(1:N,2),'y');hold; plot(t(1:N),tmeas2(1:N,2),'g');ylabel('俯仰角/rad');
subplot(322);plot(t(1:N),measR2(1:N,1),'y');hold; plot(t(1:N),tmeas2(1:N,1),'g');ylabel('方位角/rad');
subplot(324);plot(t(1:N),measR2(1:N,2),'y');hold; plot(t(1:N),tmeas2(1:N,2),'g'); ylabel('俯仰角/rad');
subplot(326);plot(t(1:N),measR2(1:N,3),'y');hold; plot(t(1:N),tmeas2(1:N,3),'g');ylabel('径向距离/m');
%图4：
figure(4)
%目标1，2融合值与真值的绝对误差
%A
subplot(321),hold;plot(t,mean(aberror1mf_x)),'b';plot(t,mean(aberror1svf_x),'r');
subplot(323),hold;plot(t,mean(aberror1mf_y)),'b';plot(t,mean(aberror1svf_y),'r');
subplot(325),hold;plot(t,mean(aberror1mf_z)),'b';plot(t,mean(aberror1svf_z),'r');
%B
subplot(322),hold;plot(t,mean(aberror2mf_x)),'y';plot(t,mean(aberror2svf_x),'g');
subplot(324),hold;plot(t,mean(aberror2mf_y)),'y';plot(t,mean(aberror2svf_y),'g');
subplot(326),hold;plot(t,mean(aberror2mf_z)),'y';plot(t,mean(aberror2svf_z),'g');
%图5：
%目标1，2融合值的位置(径向距离）标准差
figure(5)
subplot(211),hold;plot(t,mean(standard1varmf.P),'b');plot(t,mean(standard1varsvf.P),'r');title('目标1位置融合值标准差')
subplot(212),hold;plot(t,mean(standard2varmf.P),'y');plot(t,mean(standard2varsvf.P),'g');title('目标2位置融合值标准差')
%图6，目标1，2融合值与真值的三方向误差方差之和
figure(6)
subplot(211),hold;plot(t,mean(Varmf1),'b');plot(t,mean(Varsvf1),'r');title('目标1误差方差之和')
subplot(212),hold;plot(t,mean(Varmf2),'y');plot(t,mean(Varsvf2),'g');title('目标2误差方差之和')
3 仿真结果
4 参考文献
[1]李世忠, 王国宏, 吴巍,等. IMM-EKF雷达与红外序贯滤波跟踪机动目标[J]. 火力与指挥控制, 2012, 37(1):5.
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。
部分理论引用网络文献，若有侵权联系博主删除。
