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;
3 仿真结果
4 参考文献
[1]向易, 汪毅, 张佳琛, 蔡怀宇, & 陈晓冬. (2019). 基于无损卡尔曼滤波的车载双雷达目标位置估计方法. 光电工程, 46(7), 9.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
