基于雷达和摄像机的公路车辆扩展目标跟踪

%创建场景exPath = fullfile(matlabroot，“例子”，“driving_fusion”，“主要”）;addpath(exPath) [scenario, egoVehicle, sensors] = helperCreateScenario;%创建显示对象display = helperExtendedTargetTrackingDisplay;创建动画编写器来记录图形的每一帧%动画写作。将'RecordGIF'设置为true以启用GIF写入。gifWriter = helperGIFWriter(图= display。数字…RecordGIF = false);

函数返回给定的跟踪和真相的错误。errorFcn = @(track,truth)helperExtendedTargetError(track,truth);函数返回轨迹与真值之间的距离。distFcn = @(track,truth)helperExtendedTargetDistance(track,truth);函数返回来自ground truth的id。默认的% identifier假定truth是用PlatformID标识的。在% drivingScenario, truth用一个actionid标识。truthIdFcn = @(x)[x. actorid];%创建度量对象。tem = trackErrorMetrics(…ErrorFunctionFormat =“自定义”，…EstimationErrorLabels = {的位置误差，“VelocityError”，“DimensionsError”，“YawError”}，…EstimationErrorFcn = errorFcn，…TruthIdentifierFcn = truthIdFcn);tam = trackAssignmentMetrics(…DistanceFunctionFormat =“自定义”，…AssignmentDistanceFcn = distFcn，…DivergenceDistanceFcn = distFcn，…TruthIdentifierFcn = truthIdFcn，…AssignmentThreshold = 30，…DivergenceThreshold = 35);%创建ospa度量对象。tom = trackOSPAMetric(…距离=“自定义”，…DistanceFcn = distFcn;…TruthIdentifierFcn = truthIdFcn);

trackerRunTimes = 0 (0,3);ospaMetric = 0 (0,3);%创建一个多对象跟踪器跟踪器= multiObjectTracker…FilterInitializationFcn = @helperInitPointFilter，…AssignmentThreshold = 30，…ConfirmationThreshold = [4 5]，…DeletionThreshold = 3);重置随机数生成器以获得可重复的结果种子= 2018;S = rng(种子);timeStep = 1;对于多目标跟踪器，雷达报告在自我笛卡尔框架和做%不报告速度。这允许我们对多个检测结果进行聚类%的传感器。为I = 1:6传感器{I}。HasRangeRate = false;传感器{我}。DetectionCoordinates =“身体”；结束

而advance(场景)&& ishhandle(显示。图)%获取场景时间time = scenario.SimulationTime;%收集自我车辆传感器的检测结果[detections,isValidTime] = helperDetect(sensors, egoVehicle, time);%如果有新的检测，更新跟踪器如果任何(isValidTime)对于点跟踪器，必须首先对检测进行聚类detectionClusters = helperClusterRadarDetections(detections);%更新跟踪器抽搐% confirmedTracks在场景坐标中confirmedTracks = updateTracks(tracker, detectionClusters, time);T = toc;%更新指标% a.获取地面真相groundTruth = scenario.Actors(2:end);%除Ego外全部% b.更新分配指标tam (confirmedTracks groundTruth);[trackid, truthid] = currentAssignment(tam);% c.更新错误度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d.更新ospa度量ospaMetric(timeStep,1) = tom(confirmedTracks, groundTruth);%更新鸟瞰图将轨道转换为自我坐标以显示confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks);显示(egoVehicle、传感器、检测、confirmedTracksEgo、detectionClusters);drawnow;%记录跟踪器运行时间trackerRunTimes(timeStep,1) = t;timeStep = timeStep + 1;捕获动画帧gifWriter ();结束结束捕获累积跟踪度量。误差度量显示了平均值模拟误差的%值。assignmentMetricsMOT = tam.trackMetricsTable;errorMetricsMOT = tem. culativetruthmetrics;如果请求，写入GIFwriteAnimation (gifWriter“multiObjectTracking”）;

释放并重新启动所有对象。重启(场景);释放(tem);释放(tam);% trackerPHD不罚tam。AssignmentThreshold = tam。AssignmentThreshold - 2;释放(显示);显示。PlotClusteredDetection = false;gifWriter。pFrames = {};为I = 1: nummel(传感器)释放(传感器{I});如果1 <= 6个传感器{I}。HasRangeRate = true;传感器{我}。DetectionCoordinates =“球形传感器”；结束结束%恢复随机种子。rng(种子)%设置传感器配置%设定自我姿势egoPose。Position = egoVehicle.Position;egoPose。Velocity = egoVehicle.Velocity;egoPose。定向=四元数([egoVehicle.]偏航egoVehicle。egoVehicle。卷),“eulerd”，“ZYX股票”，“帧”)，“帧”）;sensorConfigurations = cell(numel(sensors)，1);为i = 1: nummel (sensors) sensorConfigurations{i} = trackingSensorConfiguration(sensors{i}，egoPose，…FilterInitializationFcn = @helperInitGGIWFilter，…SensorTransformFcn = @ctmeas);结束

partFcn = @(x)helperPartitioningFcn(x,3,5);跟踪器= trackerPHD(传感器配置=传感器配置)…PartitioningFcn = partFcn，…AssignmentThreshold = 450，…%一个检测单元(每个单元多次检测)添加出生成分的最小负对数可能性。extracactionthreshold = 0.75，…%要声明为轨道的过滤器组件的权重阈值ConfirmationThreshold = 0.85，…%要声明为已确认轨道的过滤器组件的权重阈值MergingThreshold = 50，…%组件合并阈值HasSensorConfigurationsInput = true…跟踪在场景框架中执行，因此传感器配置随时间变化）;

timeStep = 1;%运行场景而advance(场景)&& ishhandle(显示。图)%获取场景时间time = scenario.SimulationTime;在自我车辆坐标中获得其他车辆的姿态ta = targetpose (egoVehicle);%收集自我车辆传感器的检测结果[detections, isValidTime, configurations] = helperDetect(sensors, egoVehicle, time);用所有检测更新跟踪器。请注意，没有%需要在将检测结果传递给跟踪器之前对它们进行聚类。此外，传感器配置作为输入传递给%追踪。抽搐% confirmedTracks在场景坐标中confirmedTracks =跟踪器(检测，配置，时间);T = toc;%更新指标% a.获取地面真相groundTruth = scenario.Actors(2:end);%除Ego外全部% b.更新分配指标tam (confirmedTracks groundTruth);[trackid, truthid] = currentAssignment(tam);% c.更新错误度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d.更新ospa度量ospaMetric(timeStep,2) = tom(confirmedTracks, groundTruth);%更新鸟瞰图将轨道转换为自我坐标以显示confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks);显示(egoVehicle、传感器、检测、confirmedTracksEgo);drawnow;%记录跟踪器运行时间trackerRunTimes(timeStep,2) = t;timeStep = timeStep + 1;%捕获GIF帧gifWriter ();结束捕获事实并跟踪度量表assignmentMetricsGGIWPHD = tam.trackMetricsTable;errorMetricsGGIWPHD = tem. culativetruthmetrics;如果请求，写入GIFwriteAnimation (gifWriter“ggiwphdTracking”）;

为i = 1: nummel (sensorConfigurations) sensorConfigurations{i}。FilterInitializationFcn = @ helperinitrectangle filter;%初始化矩形目标gmphdsensorConfigurations{}。SensorTransformFcn= @ctrectcorners;%使用角来计算检测概率结束使用新的传感器配置定义跟踪器跟踪器= trackerPHD(传感器配置=传感器配置)…PartitioningFcn = partFcn，…AssignmentThreshold = 600，…%最小负对数-检测细胞添加出生成分的可能性extracactionthreshold = 0.85，…%要声明为轨道的过滤器组件的权重阈值ConfirmationThreshold = 0.95，…%要声明为已确认轨道的过滤器组件的权重阈值MergingThreshold = 50，…%组件合并阈值HasSensorConfigurationsInput = true…跟踪在场景框架中执行，因此传感器配置随时间变化）;释放并重新启动所有对象。重启(场景);为I = 1: nummel(传感器)释放(传感器{I});结束释放(tem);释放(tam);释放(显示);显示。PlotClusteredDetection = false;gifWriter。pFrames = {};%恢复随机种子。rng(种子)%第一时间步长timeStep = 1;%运行场景而advance(场景)&& ishhandle(显示。图)%获取场景时间time = scenario.SimulationTime;在自我车辆坐标中获得其他车辆的姿态ta = targetpose (egoVehicle);%收集自我车辆传感器的检测结果[detections, isValidTime, configurations] = helperDetect(sensors, egoVehicle, time);用所有检测更新跟踪器。请注意，没有%需要在将检测结果传递给跟踪器之前对它们进行聚类。此外，传感器配置作为输入传递给%追踪。抽搐% confirmedTracks在场景坐标中confirmedTracks =跟踪器(检测，配置，时间);T = toc;%更新指标% a.获取地面真相groundTruth = scenario.Actors(2:end);%除Ego外全部% b.更新分配指标tam (confirmedTracks groundTruth);[trackid, truthid] = currentAssignment(tam);% c.更新错误度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d.更新ospa度量ospaMetric(timeStep,3) = tom(confirmedTracks, groundTruth);%更新鸟瞰图将轨道转换为自我坐标以显示confirmedTracksEgo = helperConvertToEgoCoordinates(egoVehicle, confirmedTracks);显示(egoVehicle、传感器、检测、confirmedTracksEgo);drawnow;%记录跟踪器运行时间trackerRunTimes(timeStep,3) = t;timeStep = timeStep + 1;%捕获GIF帧gifWriter ();结束捕获事实并跟踪度量表assignmentMetricsGMPHD = tam.trackMetricsTable;errorMetricsGMPHD = tem. culativetruthmetrics;如果请求，写入GIFwriteAnimation (gifWriter“gmphdTracking”）;将随机数生成器返回到之前的状态rng (S) rmpath (exPath)

helperPlotAssignmentMetrics(assignmentMetricsMOT, assignmentMetricsGGIWPHD, assignmentMetricsGMPHD);

helpploterrormetrics (errorMetricsMOT, errorMetricsGGIWPHD, errorMetricsGMPHD);

ospaFig =图;情节(ospaMetric“线宽”2);传奇(“点目标跟踪器”，“GGIW-PHD追踪”，“矩形GM-PHD追踪器”）;包含(时间步长(k)）;ylabel (“OSPA”）;

runTimeFig =图;h = plot(trackerRunTimes(3:end，:)./trackerRunTimes(3:end,1)，“线宽”2);传奇(“点目标跟踪器”，“GGIW-PHD追踪”，“矩形GM-PHD追踪器”）;包含(时间步长(k)）;ylabel (' $ $ \压裂{t_{追踪}识别}{t_ {multiObjectTracker}识别}$ $”，“翻译”，“乳胶”，“字形大小”14);ylim([0 max([h.YData]) + 1]);