为车辆1创建跟踪器v1Tracker = trackerJPDA (“TrackerIndex”,1，“DeletionThreshold”(4 - 4),“AssignmentThreshold”, 100年正);%车辆1跟踪器posSelector = [1 0 0 0 0 0;0 0 1 0 0 0];

定义每辆车的来源v1TrackerConfiguration = fuserSourceConfiguration (“SourceIndex”,1，“iInternalSource”,真的,.．.% v1Tracker是v1Fuser的内部“CentralToLocalTransformFcn”@scenarioToEgo,“LocalToCentralTransferorMFCN”，@egotosecenario）；%坐标变换v2FuserConfiguration = fuserSourceConfiguration (“SourceIndex”，4，“iInternalSource”、假);% v2Fuser在v2Fuser之外v1Sources = {v1TrackerConfiguration;v2FuserConfiguration};v2TrackerConfiguration = fuserSourceConfiguration (“SourceIndex”，2，“iInternalSource”,真的,.．.% v2Tracker是v2Fuser的内部“CentralToLocalTransformFcn”@scenarioToEgo,“LocalToCentralTransferorMFCN”，@egotosecenario）；%坐标变换v1FuserConfiguration = fuserSourceConfiguration (“SourceIndex”3,“iInternalSource”、假);%v1Fuser位于v2Fuser的外部v2Sources = {v2TrackerConfiguration;v1FuserConfiguration};

stateParams =结构(“帧”,“矩形”,“位置”(0 0 0),“速度”, 0 0 0);v1Fuser = trackFuser (“FuserIndex”3,.．.“AssignmentThreshold”, 100年正无穷,.．.“MaxNumSources”，2，“SourceConfigurations”v1Sources,.．.“StateFusion”,“十字路口”,“DeletionThreshold”,[3 3],.．.“状态参数”，stateParams）；v2Fuser=跟踪定影器(“FuserIndex”，4，.．.“AssignmentThreshold”, 100年正无穷,.．.“MaxNumSources”，2，“SourceConfigurations”v2Sources,“StateFusion”,.．.“十字路口”,“DeletionThreshold”,[3 3],.．.“状态参数”，stateParams）；%初始化以下变量fusedTracks1 = objectTrack.empty (0,1);fusedTracks2 = objectTrack.empty (0,1);wasFuser1Updated = false;wasFuser2Updated = false;

%创建drivingScenario对象和两辆车[scenario, vehicle1, vehicle2] = createDrivingScenario;%创建所有传感器[sensors, numSensors, attachdvehicle] = createSensors(scenario);%创建显示[f，绘图仪]= createV2VDisplay(场景，传感器，附属车辆);

将每辆车定义为一个参与者、传感器、跟踪器和绘图仪的组合v1=结构(“演员”{vehicle1},“传感器”,{传感器(attachedVehicle = = 1)},“追踪”，{v1Tracker}，“DetPlotter”{plotters.veh1DetPlotter},“TrkPlotter”，{plotters.veh1TrkPlotter}）；v2=struct(“演员”{vehicle2},“传感器”,{传感器(attachedVehicle = = 2)},“追踪”{{}},“DetPlotter”{{}},“TrkPlotter”, {plotters.veh2TrkPlotter});%车辆2上没有侦测到或追踪器

运行=真；%对于可重复的结果，设置随机数种子s=rng；rng（2019）快照时间=[0.5,2.8,4.4,6.3，inf]；快照=单元（numel（快照时间，1））；i=1；f.可见=“开”;而运行&& ishhandle (f) time = scenario.SimulationTime;%检测和跟踪车辆水平[tracks1, wasTracker1Updated] = detectAndTrack (v1、时间posSelector);[tracks2, wasTracker2Updated] = detectAndTrack (v2、时间posSelector);%保留以前fuser更新的轨道oldFusedTracks1=fusedTracks1；oldFusedTracks2=fusedTracks2；%更新熔断器如果WasTracker1更新| | WasFuser 2更新的tracksToFuse1=[tracks1；oldFusedTracks2]；如果isLocked(v1Fuser) || ~isempty(tracksToFuse1) [fusedTracks1，~，~，info1] = v1Fuser(tracksToFuse1,time);wasFuser1Updated = true;pos = getTrackPositions (fusedTracks1 posSelector);字符串id = ((fusedTracks1.TrackID) ');plotTrack (plotters.veh1FusePlotter、pos、ids);其他的wasFuser1Updated=false；fusedTracks1=objectTrack.empty（0,1）；终止其他的wasFuser1Updated=false；fusedTracks1=objectTrack.empty（0,1）；终止如果wasstracker2updated || wasFuser1Updated tracksToFuse2 = [tracks2;oldFusedTracks1];如果isLocked（v2Fuser）| | ~isempty（tracksToFuse2）[fusedTracks2，~，~，info2]=v2Fuser（tracksToFuse2，time）；wasFuser2Updated=true；pos=getTrackPositions（fusedTracks2，posSelector）；id=string（[fusedTracks2.TrackID]）；plotrack（plotters.veh2FusePlotter，pos，ids）；其他的wasFuser2Updated = false;fusedTracks2 = objectTrack.empty (0,1);终止其他的wasFuser2Updated = false;fusedTracks2 = objectTrack.empty (0,1);终止%更新显示updateV2VDisplay(绘图仪，场景，传感器，附属车辆)%将场景向前推进一步，如果场景完成，则退出循环运行=进步(场景);%在指定时间捕获帧的图像如果Time >= snaptimes(i) snaps{i} = takesnap(f);I = I + 1;终止终止

showsnap(四合扣,1)

showsnap(四合扣,2)

showsnap(四合扣,3)

showsnap(断了,4)

%重新启动驱动场景，使角色返回到初始位置重启（场景）；释放所有的传感器对象，这样它们可以再次使用对于sensorIndex = 1:numSensors release(sensors{sensorIndex});终止%将随机种子返回到其以前的值rng (s)

函数[scenario, egoVehicle, secondVehicle] = createDrivingScenario%构造一个驱动场景对象场景= drivingScenario (“SampleTime”, 0.1);%添加所有路段道路中心= [50.8 0.5 0;253.4 - 1.5 0);roadWidth = 12;路(场景、roadCenters roadWidth);道路中心= [100.7 -100.6 0;100.7 - 103.7 0);路(场景,roadCenters);道路中心= [201.1 -99.2 0;199.7 - 99.5 0);路(场景,roadCenters);添加自我载体egoVehicle =车辆(场景中,“ClassID”, 1“位置”， [65.1 -0.9 0]，“PlotColor”， [0 0.7410 0.4470]);路径点= [71 -0.5 0;148.7 - -0.5 0);速度= 12;轨迹(egoVehicle、锚点、速度);%添加第二辆车第二辆车=车辆（场景，“ClassID”, 1“位置”， [55.1 -0.9 0]);路径点= [61 -0.5 0;138.7 - -0.5 0);速度= 12;轨迹(secondVehicle、锚点、速度);把停着的车加起来车辆(场景中,“ClassID”, 1“位置”， [111.0 -3.6 0]);车辆(场景中,“ClassID”, 1“位置”， [140.6 -3.6 0]);车辆(场景中,“ClassID”, 1“位置”，[182.6-3.6 0]）；车辆（场景，“ClassID”, 1“位置”， [211.3 -4.1 0]);%增加行人演员(场景中,“ClassID”4“长度”, 0.5,“宽度”, 0.5,.．.“高度”, 1.7,“位置”， [130.3 -2.7 0]，“RCSPattern”， [-8 -8]);%添加停放的卡车车辆(场景中,“ClassID”2,“长度”, 8.2,“宽度”, 2.5,.．.“高度”, 3.5,“位置”， [117.5 -3.5 0]);终止

函数[sensors，numSensors，attachedVehicle]=创建传感器（场景）%createSensors返回所有传感器对象以生成检测%在createssensors和createDrivingScenario中使用的单位%距离/位置-米%速度-米/秒角-度% RCS模式- dBsm%为每个传感器分配所有参与者的物理和雷达剖面概要文件= actorProfiles(场景);%车辆1雷达报告集群检测传感器{1}= radarDataGenerator (“没有扫描”,“SensorIndex”, 1“UpdateRate”10.．.“MountingLocation”， [3.7 0 0.2]，“RangeLimits”, [0 50],“FieldOfView”, [60 5],.．.“RangeResolution”, 2.5,“AzimuthResolution”4.．.“配置文件”配置文件,“HasOcclusion”,真的,“HasFalseAlarms”假的,.．.“TargetReportFormat”,“集群检测”)；车辆2雷达跟踪传感器{2}=雷达数据发生器(“没有扫描”,“SensorIndex”2,“UpdateRate”10.．.“MountingLocation”， [3.7 0 0.2]，“RangeLimits”, [0 120],“FieldOfView”30 [5],.．.“RangeResolution”, 2.5,“AzimuthResolution”4.．.“配置文件”配置文件,“HasOcclusion”,真的,“HasFalseAlarms”假的,.．.“TargetReportFormat”,“轨道”,“DeletionThreshold”3 [3]);%车辆1视觉传感器报告检测到传感器{3}= visionDetectionGenerator (“SensorIndex”3,.．.“MaxRange”, 100,“SensorLocation”, [1.9 0],“DetectorOutput”,“只将对象”,.．.“ActorProfiles”,概要文件);attachedVehicle = [1, 2, 1];numSensors =元素个数(传感器);终止

函数trackInEgo=scenarioToEgo（trackInScenario）egoPosInScenario=trackInScenario.StateParameters.OriginPosition；egoVelInScenario=trackInScenario.StateParameters.OriginVelocity；stateInScenario=trackInScenario.State；stateShift=[egoPosInScenario（1）；egoVelInScenario（1）；egoPosInScenario（2）；egoVelInScenario（2）；egoPosInScenario）（3） ]；stateInEgo=stateInScenario-stateShift；trackInEgo=objectTrack(“UpdateTime”, trackInScenario。UpdateTime,“状态”，stateInEgo，“StateCovariance”, trackInScenario。StateCovariance,“状态参数”, trackInScenario.StateParameters);终止

函数trackInScenario = egoToScenario(trackInEgo) egoPosInScenario = trackInEgo. stateparameters . originposition;egoVelInScenario = trackInEgo.StateParameters.OriginVelocity;stateInScenario = trackInEgo.State;stateShift = [egoPosInScenario (1); egoVelInScenario (1); egoPosInScenario (2); egoVelInScenario (2); egoPosInScenario (3); egoVelInScenario (3)];statinego = statinscenario + statshift;trackInScenario = objectTrack (“UpdateTime”, trackInEgo。UpdateTime,“状态”，stateInEgo，“StateCovariance”, trackInEgo。StateCovariance,“状态参数”, trackInEgo.StateParameters);终止

函数[追踪,wasTrackerUpdated] = detectAndTrack(代理,时间,posSelector)%从车辆中创建检测姿势=目标姿势（agent.Actor）；[detections，isValid]=车辆检测（agent.Actor.Position，agent.Sensors，poses，time，agent.DetPlotter）；%更新跟踪器，从报告检测的传感器获取轨迹如果isValid agent.Tracker.StateParameters = struct(.．.“帧”,“矩形”,.．.“OriginPosition”agent.Actor.Position,.．.“OriginVelocity”，agent.Actor.Velocity）；tracks=agent.Tracker（检测，时间）；tracksInScenario=tracks；对于i = 1:numel(tracks) tracksInScenario(i) = egoToScenario(tracks(i));终止pos = getTrackPositions (tracksInScenario posSelector);plotTrack(agent.TrkPlotter,pos) wasstrackerupdated = true;其他的跟踪= objectTrack.empty (0,1);wasTrackerUpdated = false;终止%从跟踪传感器获取其他轨迹[sensorTracks, wasSensorTrackerUpdated] = vehicleTracks (agent.Actor agent.Sensors,姿势、时间agent.TrkPlotter);跟踪= vertcat(跟踪、sensorTracks);wasstrackerupdated = wasstrackerupdated || wasSensorTrackerUpdated;终止

函数[objectDetections,isValid] = vehicleDetections(位置，传感器，姿势，时间，绘图仪)numSensors = nummel(传感器);objectDetections = {};isValidTime = false(1, numSensors);%为每个传感器生成检测对于sensorIndex = 1:numSensors sensor = sensors{sensorIndex};如果isa(传感器,“visionDetectionGenerator”) | | ~ strcmpi(传感器。TargetReportFormat,“轨道”) [objectDets， ~， sensorConfig] =传感器(姿势，时间);如果islogical(sensorConfig) isValidTime(sensorIndex) = sensorConfig;其他的isValidTime (sensorIndex) = sensorConfig.IsValidTime;终止setAtt(d)， objectDets = cellfun(@(d))“UniformOutput”、假);numObjects =元素个数(objectDets);objectDetections = [objectDetections;objectDets (1: numObjects)];% #好< AGROW >终止终止isValid = (isValidTime);%绘制检测如果numel(objectDetections)>0 detPos = cellfun(@(d)d. measurement (1:2)， objectDetections，“UniformOutput”、假);detPos = cell2mat(detPos')' + position(1:2);plotDetection(绘图仪,detPos);终止终止函数d=设定值（d）%设置属性为一个结构d、 ObjectAttributes=struct；%只保留位置测量和移除速度如果nummel (d.Measurement)==6;d.MeasurementNoise = d.MeasurementNoise (1:3, 1:3);d.MeasurementParameters{1}。HasVelocity = false;终止终止

函数[轨迹，wasTrackerUpdated]=车辆轨迹（演员、传感器、姿势、时间、绘图仪）%从车辆中创建检测numSensors =元素个数(传感器);跟踪= objectTrack.empty;isValidTime = false(1, numSensors);%为每个传感器生成检测对于sensorIndex = 1:numSensors sensor = sensors{sensorIndex};如果isa(传感器,“radarDataGenerator”) & & strcmpi(传感器。TargetReportFormat,“轨道”) [sensorTracks， ~， sensorConfig] =传感器(姿势，时间);如果islogical(sensorConfig) isValidTime(sensorIndex) = sensorConfig;其他的isValidTime (sensorIndex) = sensorConfig.IsValidTime;终止numObjects =元素个数(sensorTracks);跟踪=[跟踪;sensorTracks (1: numObjects)];% #好< AGROW >终止终止wasTrackerUpdated = (isValidTime);如果~ wasTrackerUpdated没有更新车辆跟踪传感器返回终止%将车辆位置和速度添加到跟踪状态参数对于i = 1:numel(tracks) tracks(i). stateparameters . ..StateParameters OriginPosition =歌曲(我)。OriginPosition + actor.Position ';跟踪.StateParameters(我)。.StateParameters OriginVelocity =歌曲(我)。OriginVelocity + actor.Velocity ';终止%的阴谋跟踪如果numel(tracks)>0 trPos = arrayfun(@(t)t. state ([1,3]))， tracks，“UniformOutput”、假);trPos = cell2mat(trPos')' + actor.Position(1:2);plotTrack(绘图仪,trPos);终止终止