目录
💥1 概述
📚2 运行结果
🎉3 参考文献
👨💻4 Matlab代码
本文使用MATLAB的场景生成器工具箱，通过合成雷达和视觉观察创建一个简单的高速公路驾驶场景。扩展卡尔曼滤波器已被实现以将车辆的状态传播到未来。将投影状态值与当前测量值进行比较以执行跟踪。
[1]尹晓东,刘后铭.改进的多目标多传感器数据融合相关算法[J].地质科技管理,1994(03):225-231.
主函数部分代码：
clear all
close all
clc
%% Parameters
% Assignment gate value
AssignmentThreshold = 30; % The higher the Gate value, the higher the likelihood that every track...
% will be assigned a detection.
% M/N initiation parameters
% The track is "confirmed" if after N consecutive updates at
% least M measurements are assigned to the track after the track initiation.
N = 5;
M = 4;
% Elimination threshold: The track will be deleted after EliminationTH # of updates without
% any measurement update
EliminationTH = 10; % updates
% Measurement Noise
R = [22.1 0 0 0
0 2209 0 0
0 0 22.1 0
0 0 0 2209];
% Process noise
Q= 7e-1.*eye(4);
% Performance anlysis parameters:
XScene = 80;
YScene = 40;
% PerfRadius is defined after scenario generation
%% Generate the Scenario
% Define an empty scenario.
scenario = drivingScenario;
scenario.SampleTime = 0.01; % seconds
SensorsSampleRate = 0.1; % seconds
EgoSpeed = 25; % m/s
%% Simple Scenario (Choice #1)
% Load scenario road and extract waypoints for each lane
Scenario = load('SimpleScenario.mat');
WPs{1} = Scenario.data.ActorSpecifications(2).Waypoints;
WPs{2} = Scenario.data.ActorSpecifications(1).Waypoints;
WPs{3} = Scenario.data.ActorSpecifications(3).Waypoints;
road(scenario, WPs{2}, 'lanes',lanespec(3));
% Ego vehicle (lane 2)
egoCar = vehicle(scenario, 'ClassID', 1);
egoWPs = circshift(WPs{2},-8);
path(egoCar, egoWPs, EgoSpeed);
% Car1 (passing car in lane 3)
Car1 = vehicle(scenario, 'ClassID', 1);
Car1WPs = circshift(WPs{1},0);
path(Car1, Car1WPs, EgoSpeed + 5);
% Car2 (car in lane 1)
Car2 = vehicle(scenario, 'ClassID', 1);
Car2WPs = circshift(WPs{3},-15);
path(Car2, Car2WPs, EgoSpeed -5);
% Ego follower (lane 2)
Car3 = vehicle(scenario, 'ClassID', 1);
Car3WPs = circshift(WPs{2},+5);
path(Car3, Car3WPs, EgoSpeed);
% Car4 (stopped car in lane 1)
Car4 = vehicle(scenario, 'ClassID', 1);
Car4WPs = circshift(WPs{3},-13);
path(Car4, Car4WPs, 1);
ActorRadius = norm([Car1.Length,Car1.Width]);
%---------------------------------------------------------------------------------------------
%% Waypoint generation (Choice #2)
% % Load scenario road and extract waypoints for each lane
% WPs = GetLanesWPs('Scenario3.mat');
% % Define road wtr the middle lane waypoints
% road(scenario, WPs{2}, 'lanes',lanespec(3));
% %%%%%%%%%%%% BE CAREFUL OF LANESPACE(3) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Ego vehicle (lane 2)
% egoCar = vehicle(scenario, 'ClassID', 1);
% path(egoCar, WPs{2}, EgoSpeed); % On right lane
%
% % Car1 (passing car in lane 3)
% Car1 = vehicle(scenario, 'ClassID', 1);
% WPs{1} = circshift(WPs{1},20);
% path(Car1, WPs{1}, EgoSpeed + 2);
%
% % Car2 (slower car in lane 1)
% Car2 = vehicle(scenario, 'ClassID', 1);
% WPs{3} = circshift(WPs{3},-50);
% path(Car2, WPs{3}, EgoSpeed -5);
%---------------------------------------------------------------------------------------------
%% Create a Tracker
% Create a |<matlab:doc('multiObjectTracker') multiObjectTracker>| to track
% the vehicles that are close to the ego vehicle. The tracker uses the
% |initSimDemoFilter| supporting function to initialize a constant velocity
% linear Kalman filter that works with position and velocity.
%
% Tracking is done in 2-D. Although the sensors return measurements in 3-D,
% the motion itself is confined to the horizontal plane, so there is no
% need to track the height.
tracker = multiObjectTracker('FilterInitializationFcn', @initSimDemoFilter, ...
'AssignmentThreshold', 30, 'ConfirmationParameters', [4 5]);
positionSelector = [1 0 0 0; 0 0 1 0]; % Position selector
velocitySelector = [0 1 0 0; 0 0 0 1]; % Velocity selector
%% Define Sensors and Bird's Eye Plot
sensors = SensorsConfig(egoCar,SensorsSampleRate);
BEP = createDemoDisplay(egoCar, sensors);
BEP1 = createDemoDisplay(egoCar, sensors);
%% Fusion Loop for the scenario
Tracks = [];
count = 0;
toSnap = true;
TrackerStep = 0;
time0 = 0;
currentStep = 0;
Performance.Actors.Ground = [];
Performance.Actors.EATracks = [];
Performance.Actors.MATracks = [];
Performance.MeanDistance.EA = [];
Performance.MeanDistance.MA = [];
Performance.GhostActors.EA = [];
Performance.GhostActors.MA = [];
while advance(scenario) %&& ishghandle(BEP.Parent)
currentStep = currentStep + 1;
% Get the scenario time
time = scenario.SimulationTime;
% Get the position of the other vehicle in ego vehicle coordinates
ta = targetPoses(egoCar);
% Simulate the sensors
detections = {};
isValidTime = false(1,length(sensors));
for i = 1:length(sensors)
[sensorDets,numValidDets,isValidTime(i)] = sensors{i}(ta, time);
if numValidDets
for j = 1:numValidDets
% Vision detections do not report SNR. The tracker requires
% that they have the same object attributes as the radar
% detections. This adds the SNR object attribute to vision
% detections and sets it to a NaN.
if ~isfield(sensorDets{j}.ObjectAttributes{1}, 'SNR')
sensorDets{j}.ObjectAttributes{1}.SNR = NaN;
end
end
detections = [detections; sensorDets]; %#ok<AGROW>
end
end
% Update the tracker if there are new detections
if any(isValidTime)
TrackerStep = TrackerStep + 1;
%----------------------------------------------------------------------------------------------
%-----------------------------------MATLAB Tracker----------------------------------------------
%----------------------------------------------------------------------------------------------
vehicleLength = sensors{1}.ActorProfiles.Length;
detectionClusters = clusterDetections(detections, vehicleLength);
confirmedTracks1 = updateTracks(tracker, detectionClusters, time);
%----------------------------------------------------------------------------------------------
%-----------------------------------Eatron Tracker----------------------------------------------
%----------------------------------------------------------------------------------------------
%% Cluster Detections
VehicleDim = [sensors{1}.ActorProfiles.Length, sensors{1}.ActorProfiles.Width,...
sensors{1}.ActorProfiles.Height];
[DetectionClusters] = ClusterDetections(detections, VehicleDim);
