【目标跟踪】基于拓展卡尔曼滤波实现车载激光雷达和雷达的数据融合matlab代码

1 简介2 部分代码clear;clf;dt=0.1;Data=csvread('Radar_Lidar_Data1.csv',1,1);% Data = csvread('Radar_Lidar_Data2.csv',1,1);Radar_Measurement= [];Lidar_Measurement= [];EKF_Path= [];F= [[1,0,dt,0];[0,1,0,dt];[

Matlab科研辅导帮

722人浏览 · 2021-11-01 00:02:31

Matlab科研辅导帮 · 2021-11-01 00:02:31 发布

1 简介

2 部分代码

clear;
clf;
dt = 0.1;
Data = csvread('Radar_Lidar_Data1.csv',1,1);
% Data = csvread('Radar_Lidar_Data2.csv',1,1);
Radar_Measurement = [];
Lidar_Measurement = [];
EKF_Path = [];

F = [[1, 0, dt, 0];
    [0, 1, 0, dt];
    [0, 0, 1, 0];
    [0, 0, 0, 1]];

u = 0;
B = [(dt^2)/2 (dt^2)/2 dt dt]';

   end
   
end

for i = 1:length(Radar_Measurement)
   Radar_Measurement_Cart(i,:) = [[Radar_Measurement(i,1),0];[0, Radar_Measurement(i,1)]]*[cos(Radar_Measurement(i,2));sin(Radar_Measurement(i,2))];
end

hold on;

plot(Data(:,6),Data(:,7),'linewidth', 2);
scatter(EKF_Path(:,1),EKF_Path(:,2),25,'filled','r');
scatter(Lidar_Measurement(:,1),Lidar_Measurement(:,2),5,'filled','blue');
scatter(Radar_Measurement_Cart(:,1),Radar_Measurement_Cart(:,2),5,'filled','g');

legend('Grundtruth','EKF Path result','Lidar Measurement','Radar Measurement','Location','northwest');
axis square;
hold off;