基于卡尔曼滤波的雷达光电多目标航迹融合算法matlab仿真

1.程序功能描述

基于卡尔曼滤波的雷达光电多目标航迹融合算法matlab仿真。实现2个雷达，一个光电下的双目标航迹融合。将雷达和光电传感器的数据进行融合，可以充分发挥两者的优势，提高目标跟踪的精度和可靠性。

2.测试软件版本以及运行结果展示

matlab2022a/matlab2024b版本运行

3.核心程序

...........................................................................
    %对两个雷达进行kalman滤波融合，主要对距离，俯仰，方位三个数据进行融合  
    %进行卡尔曼处理，两个雷达的扫描进行合并+包括坐标系的转换   
    K1_A1_dat = func_func_kalman_total1(S1_A1_dat,S2_A1_dat,1);
    K1_A2_dat = func_func_kalman_total1(S1_A2_dat,S2_A2_dat,1);    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %以下部分开始光电部分的处理
    if type == 3%光电
       idx3          =  idx3+1; 
       T3_time(idx3)    =  tmps(1);%时间序号
       T3_package(idx3) =  tmps(2);%数据包序号 
       T3_Aimtime(idx3) =  tmps(3);%目标时间（秒） 
       T3_theta1(idx3)  =  tmps(5);%俯仰（度） 
       T3_theta2(idx3)  =  tmps(6);%方位（度）
       %直接极坐标下进行关联
       datmp3           = [T3_time(idx3),T3_package(idx3),T3_Aimtime(idx3),T3_theta1(idx3),T3_theta2(idx3)];
       %直接极坐标下进行关联
       if idx2 > Start %消除一开始的有误差的几帧
          tmp1      = [T3_theta1(Start),T3_theta2(Start)]; 
          tmp2      = [T3_theta1(idx3) ,T3_theta2(idx3)]; 
          dst       = sqrt((tmp1(1)-tmp2(1))^2 + (tmp1(2)-tmp2(2))^2);
          if dst>LVL%关联分类
             T3_A1_dat= [T3_A1_dat;datmp3];%关联上则加入数据向量
             T3_A2_dat= T3_A2_dat;%没有关联上，则丢弃
          else
             T3_A1_dat= T3_A1_dat;%没有关联上，则丢弃
             T3_A2_dat= [T3_A2_dat;datmp3];%关联上则加入数据向量
          end
       end
    end
    %插值
    if size(T3_A1_dat,1)>=2%插值
       C3_A1_dat = func_insert(T3_A1_dat,2);  
    end
    if size(T3_A2_dat,1)>=2%插值
       C3_A2_dat = func_insert(T3_A2_dat,2);
    end
    %时间对齐同步
    S3_A1_dat = C3_A1_dat;
    S3_A2_dat = C3_A2_dat; 
    K3_A1_dat = S3_A2_dat;
    K3_A2_dat = S3_A1_dat;    
    %将这些数据进行合并到新的航迹矩阵中
    %时间对齐同步
    [RD_A1_Dat,GD_A1_Dat]=func_timesysn(K1_A1_dat,K3_A1_dat);
    [RD_A2_Dat,GD_A2_Dat]=func_timesysn(K1_A2_dat,K3_A2_dat);
    %将雷达和光电数据融合到目标对应的估计中，获得目标最终的轨迹
    A1_dat = func_func_kalman_total2(RD_A1_Dat,GD_A1_Dat);
    A2_dat = func_func_kalman_total2(RD_A2_Dat,GD_A2_Dat);
end
 
 
 
[x0,y0] = pol2cart(A1_dat(:,4),A1_dat(:,5));
x1 = [A1_dat(:,3).*x0];
y1 = [A1_dat(:,3).*y0];
 
[x0,y0] = pol2cart(A2_dat(:,4),A2_dat(:,5));
x2 = [A2_dat(:,3).*x0];
y2 = [A2_dat(:,3).*y0];
 
 
%显示算法处理前的效果
figure;
subplot(121);plot(x1,y1,'b','linewidth',3);title('目标1融合航迹');
grid on
subplot(122);plot(x2,y2,'b','linewidth',3);title('目标2融合航迹');
grid on
016_069m

4.本算法原理

在现代多传感器目标跟踪系统中，雷达和光电传感器具有各自独特的优势。雷达能够在较远的距离上对目标进行探测和跟踪，提供目标的距离、速度等信息；光电传感器则具有较高的角度测量精度和图像分辨率，能够提供目标的视觉特征。将雷达和光电传感器的数据进行融合，可以充分发挥两者的优势，提高目标跟踪的精度和可靠性。卡尔曼滤波作为一种经典的最优估计方法，在多目标航迹融合中得到了广泛的应用。