扩展对象的高速公路车辆跟踪雷达和摄像头

%创建场景exPath = fullfile (matlabroot,“例子”,“driving_fusion”,“主要”);目录exPath)(场景、egoVehicle传感器)= helperCreateScenario;%创建显示对象显示= helperExtendedTargetTrackingDisplay;%创建动画作家记录每一帧的图%动画写作。设置“RecordGIF”真让GIF写作。gifWriter = helperGIFWriter(图= display.Figure,…RecordGIF = false);

%函数返回的错误跟踪和真理。errorFcn = @(跟踪、真理)helperExtendedTargetError(跟踪、真理);%函数返回跟踪和真理之间的距离。distFcn = @(跟踪、真理)helperExtendedTargetDistance(跟踪、真理);%函数返回id从地面真理。默认的%标识符假定事实与PlatformID标识。在% drivingScenario,真理是确定ActorID。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度量对象。汤姆= trackOSPAMetric (…距离=“自定义”,…DistanceFcn = distFcn,…TruthIdentifierFcn = truthIdFcn);

trackerRunTimes = 0 (0, 3);ospaMetric = 0 (0, 3);%创建一个multiObjectTracker追踪= multiObjectTracker (…FilterInitializationFcn = @helperInitPointFilter,…AssignmentThreshold = 30,…ConfirmationThreshold = [4 - 5],…DeletionThreshold = 3);%重置的随机数字生成器可重复的结果种子= 2018;S = rng(种子);步伐= 1;% multiObjectTracker,雷达在自我笛卡尔坐标系,并报告%不报告速度。这让我们从多个集群检测%的传感器。为{我}i = 1:6传感器。HasRangeRate = false;传感器{我}。DetectionCoordinates =“身体”;结束

而推进(场景)& & ishghandle (display.Figure)%的场景时间= scenario.SimulationTime;%收集自我车辆检测传感器[检测,isValidTime] = helperDetect(传感器、egoVehicle、时间);%更新追踪是否有新的检测如果任何(isValidTime)%检测必须首先集中重点跟踪detectionClusters = helperClusterRadarDetections(检测);%更新追踪抽搐% confirmedTracks场景坐标confirmedTracks = updateTracks(跟踪、detectionClusters、时间);t = toc;%更新指标%获得地面实况groundTruth = scenario.Actors(2:结束);%除了自我% b。更新任务指标tam (confirmedTracks groundTruth);[trackIDs, truthIDs] = currentAssignment (tam);% c。更新误差度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d。更新ospa度量汤姆ospaMetric(步伐,1)= (confirmedTracks groundTruth);%更新鸟's-eye-plot%转换跟踪自我坐标显示confirmedTracksEgo = helperConvertToEgoCoordinates (egoVehicle confirmedTracks);显示器(egoVehicle、传感器、检测、confirmedTracksEgo detectionClusters);drawnow;%记录追踪运行时间trackerRunTimes(步伐,1)= t;步伐=步伐+ 1;%捕捉帧动画gifWriter ();结束结束%捕获累积跟踪指标。显示平均误差指标%值的误差仿真。assignmentMetricsMOT = tam.trackMetricsTable;errorMetricsMOT = tem.cumulativeTruthMetrics;%如果要求写GIFwriteAnimation (gifWriter“multiObjectTracking”);

%释放和重新启动所有对象。重启(场景);释放(tem);释放(tam);%没有penality trackerPHDtam。AssignmentThreshold = tam。AssignmentThreshold - 2;释放(显示);显示。PlotClusteredDetection = false;gifWriter。pFrames = {};为i = 1:元素个数(传感器)释放(传感器{我});如果{我}我< = 6传感器。HasRangeRate = true;传感器{我}。DetectionCoordinates =“球形传感器”;结束结束%恢复随机种子。rng(种子)%设置传感器配置%设置自我构成egoPose。位置= egoVehicle.Position;egoPose。速度= egoVehicle.Velocity;egoPose。取向= rotmat(四元数([egoVehicle。偏航egoVehicle。距egoVehicle.Roll),“eulerd”,“ZYX股票”,“帧”),“帧”);sensorConfigurations =细胞(元素个数(传感器),1);为i = 1:元素个数(传感器)sensorConfigurations{我}= trackingSensorConfiguration(传感器{我}、egoPose…FilterInitializationFcn = @helperInitGGIWFilter,…SensorTransformFcn = @ctmeas);结束

partFcn = @ (x) helperPartitioningFcn (x, 3、5);追踪= trackerPHD (SensorConfigurations = SensorConfigurations,…PartitioningFcn = partFcn,…AssignmentThreshold = 450,…%的最小负对数似检测细胞出生(多个检测每个细胞)来添加组件。ExtractionThreshold = 0.75,…%重量阈值过滤组件的宣布道ConfirmationThreshold = 0.85,…%重量阈值的过滤器组件宣布确认跟踪MergingThreshold = 50,…%阈值合并组件HasSensorConfigurationsInput = true…%在场景帧进行跟踪,因此传感器配置随时间变化);

步伐= 1;%运行场景而推进(场景)& & ishghandle (display.Figure)%的场景时间= scenario.SimulationTime;%的姿势在自我车辆坐标系中其他车辆ta = targetPoses (egoVehicle);%收集自我车辆检测传感器(检测、isValidTime、配置)= helperDetect(传感器、egoVehicle、时间);%更新追踪检测。注意,没有%需要集群检测之前通过追踪。%,作为输入传递到传感器配置%追踪。抽搐% confirmedTracks场景坐标confirmedTracks =追踪(检测、配置时间);t = toc;%更新指标%获得地面实况groundTruth = scenario.Actors(2:结束);%除了自我% b。更新任务指标tam (confirmedTracks groundTruth);[trackIDs, truthIDs] = currentAssignment (tam);% c。更新误差度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d。更新ospa度量汤姆ospaMetric(步伐,2)= (confirmedTracks groundTruth);%更新鸟瞰的阴谋%转换跟踪自我坐标显示confirmedTracksEgo = helperConvertToEgoCoordinates (egoVehicle confirmedTracks);显示器(egoVehicle、传感器、检测、confirmedTracksEgo);drawnow;%记录追踪运行时间trackerRunTimes(步伐,2)= t;步伐=步伐+ 1;%为GIF捕获帧gifWriter ();结束%捕捉真相和跟踪指标表assignmentMetricsGGIWPHD = tam.trackMetricsTable;errorMetricsGGIWPHD = tem.cumulativeTruthMetrics;%如果要求写GIFwriteAnimation (gifWriter“ggiwphdTracking”);

为i = 1:元素个数(sensorConfigurations) sensorConfigurations {}。FilterInitializationFcn = @helperInitRectangularFilter;%初始化一个矩形目标gmphdsensorConfigurations {}。SensorTransformFcn= @ctrectcorners;%使用角计算探测概率结束%定义跟踪使用新的传感器配置追踪= trackerPHD (SensorConfigurations = SensorConfigurations,…PartitioningFcn = partFcn,…AssignmentThreshold = 600,…%的最小负对数似检测细胞出生添加组件ExtractionThreshold = 0.85,…%重量阈值过滤组件的宣布道ConfirmationThreshold = 0.95,…%重量阈值的过滤器组件宣布确认跟踪MergingThreshold = 50,…%阈值合并组件HasSensorConfigurationsInput = true…%在场景帧进行跟踪,因此传感器配置随时间变化);%释放和重新启动所有对象。重启(场景);为i = 1:元素个数(传感器)释放(传感器{我});结束释放(tem);释放(tam);释放(显示);显示。PlotClusteredDetection = false;gifWriter。pFrames = {};%恢复随机种子。rng(种子)%时间的第一步步伐= 1;%运行场景而推进(场景)& & ishghandle (display.Figure)%的场景时间= scenario.SimulationTime;%的姿势在自我车辆坐标系中其他车辆ta = targetPoses (egoVehicle);%收集自我车辆检测传感器(检测、isValidTime、配置)= helperDetect(传感器、egoVehicle、时间);%更新追踪检测。注意,没有%需要集群检测之前通过追踪。%,作为输入传递到传感器配置%追踪。抽搐% confirmedTracks场景坐标confirmedTracks =追踪(检测、配置时间);t = toc;%更新指标%获得地面实况groundTruth = scenario.Actors(2:结束);%除了自我% b。更新任务指标tam (confirmedTracks groundTruth);[trackIDs, truthIDs] = currentAssignment (tam);% c。更新误差度量tem (confirmedTracks trackIDs、groundTruth truthIDs);% d。更新ospa度量汤姆ospaMetric(步伐,3)= (confirmedTracks groundTruth);%更新鸟瞰的阴谋%转换跟踪自我坐标显示confirmedTracksEgo = helperConvertToEgoCoordinates (egoVehicle confirmedTracks);显示器(egoVehicle、传感器、检测、confirmedTracksEgo);drawnow;%记录追踪运行时间trackerRunTimes(步伐,3)= t;步伐=步伐+ 1;%为GIF捕获帧gifWriter ();结束%捕捉真相和跟踪指标表assignmentMetricsGMPHD = tam.trackMetricsTable;errorMetricsGMPHD = tem.cumulativeTruthMetrics;%如果要求写GIFwriteAnimation (gifWriter“gmphdTracking”);%随机数生成器回到之前的状态rng (S) rmpath (exPath)

helperPlotAssignmentMetrics (assignmentMetricsMOT assignmentMetricsGGIWPHD assignmentMetricsGMPHD);

helperPlotErrorMetrics (errorMetricsMOT errorMetricsGGIWPHD errorMetricsGMPHD);

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

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