使用合成雷达和视觉传感器融合数据

%定义一个空的场景。场景= drivingScenario;场景。SampleTime = 0.01;

roadCenters = [0 0;50 0;100 0;250 20;500年40];mainRoad =路(场景、roadCenters“道”lanespec (2));障碍(场景,mainRoad);

%创建自我车辆,以沿着路25米/秒。把%在右车道车辆减去半个车道宽度(1.8米)%的中心线的道路。egoCar =车辆(场景中,“ClassID”1);轨迹(egoCar roadCenters 2:,: - 1.8 [0], 25);%在右车道%添加一个车里的自我leadCar =车辆(场景中,“ClassID”1);轨迹(leadCar [70 0;roadCenters (3:,:)] - [0 1.8], 25);%在右车道%添加一个车沿着路以35米/秒的速度传播,并将自我passingCar =车辆(场景中,“ClassID”1);路点= [0 -1.8;50 1.8;100年1.8;250年21.8;400年32.2;500年38.2);轨迹(passingCar锚点35);%自我后面添加一个辆车车chaseCar =车辆(场景中,“ClassID”1);轨迹(chaseCar [25 0;roadCenters (2:,:)] - [0 1.8], 25);%在右车道

传感器=细胞(8,1);%前置远程雷达传感器在车的前保险杠的中心。传感器{1}= drivingRadarDataGenerator (“SensorIndex”, 1“RangeLimits”174年[0],…“MountingLocation”,[egoCar。轴距+ egoCar。FrontOverhang, 0, 0.2),“FieldOfView”5],(20日);%后置远程雷达传感器在汽车的后保险杠的中心。传感器{2}= drivingRadarDataGenerator (“SensorIndex”2,“MountingAngles”(180 0 0),…“MountingLocation”,[-egoCar。RearOverhang, 0, 0.2),“RangeLimits”,30 [0],“FieldOfView”5],(20日);% Rear-left-facing短程雷达传感器在车的左后轮好。传感器{3}= drivingRadarDataGenerator (“SensorIndex”3,“MountingAngles”(120 0 0),…“MountingLocation”,[0,egoCar。宽/ 2 0.2),“RangeLimits”,30 [0],“ReferenceRange”,50岁,…“FieldOfView”(90 5),“AzimuthResolution”10“RangeResolution”,1.25);% Rear-right-facing短程雷达传感器,在车的右后轮舱。传感器{4}= drivingRadarDataGenerator (“SensorIndex”4“MountingAngles”(-120 0 0),…“MountingLocation”,[0,-egoCar。宽/ 2 0.2),“RangeLimits”,30 [0],“ReferenceRange”,50岁,…“FieldOfView”(90 5),“AzimuthResolution”10“RangeResolution”,1.25);% Front-left-facing短程雷达传感器在车的左前轮好。{5}= drivingRadarDataGenerator(传感器“SensorIndex”5,“MountingAngles”(60 0 0),…“MountingLocation”,[egoCar。轴距,egoCar。宽/ 2 0.2),“RangeLimits”,30 [0],…“ReferenceRange”,50岁,“FieldOfView”(90 5),“AzimuthResolution”10…“RangeResolution”,1.25);% Front-right-facing短程雷达传感器在正确的汽车的前轮。{6}= drivingRadarDataGenerator(传感器“SensorIndex”6“MountingAngles”(-60 0 0),…“MountingLocation”,[egoCar。轴距,-egoCar。宽/ 2 0.2),“RangeLimits”,30 [0],…“ReferenceRange”,50岁,“FieldOfView”(90 5),“AzimuthResolution”10…“RangeResolution”,1.25);%前置摄像头位于前挡风玻璃。{7}= visionDetectionGenerator(传感器“SensorIndex”7“FalsePositivesPerImage”,0.1,…“SensorLocation”(0.75 * egoCar。轴距0),“高度”,1.1);%后置摄像头位于后方挡风玻璃。传感器{8}= visionDetectionGenerator (“SensorIndex”8“FalsePositivesPerImage”,0.1,…“SensorLocation”(0.2 * egoCar。轴距0),“高度”,1.1,“偏航”,180);%注册演员资料与传感器。概要文件= actorProfiles(场景);为m = 1:元素个数(传感器)如果isa(传感器{m},“drivingRadarDataGenerator”{m})传感器。概要文件=概要;其他的传感器{m}。ActorProfiles =概要;结束结束

追踪= multiObjectTracker (“FilterInitializationFcn”@initSimDemoFilter,…“AssignmentThreshold”30岁的“ConfirmationThreshold”[4 - 5]);positionSelector = [1 0 0 0;0 0 1 0);%的位置选择器velocitySelector = [0 1 0 0;0 0 0 1);%的速度选择器%创建显示并返回一个句柄鸟瞰的阴谋cep = createDemoDisplay (egoCar、传感器);

toSnap = true;而推进(场景)& & ishghandle (BEP.Parent)%的场景时间= scenario.SimulationTime;%得到其他车辆在自我车辆的位置坐标ta = targetPoses (egoCar);%模拟传感器detectionClusters = {};isValidTime = false (8);为我= 1:8 (sensorDets、numValidDets isValidTime (i)) =传感器{我}(助教、时间);如果numValidDets为j = 1: numValidDets%的视觉检测信噪比不报告。跟踪要求%,他们有相同的对象属性的雷达%检测。这增加了视觉信噪比对象属性%检测和集南。如果~ isfield (sensorDets {j} .ObjectAttributes {1},“信噪比”)sensorDets {j} .ObjectAttributes {1}。信噪比=南;结束% z分量的测量位置和速度%的测量和MeasurementNoise字段sensorDets {j}。测量= sensorDets {j}。测量([1 2 4 5]);sensorDets {j}。MeasurementNoise = sensorDets {j}。MeasurementNoise ([1 2 4 5]、[1 2 4 5]);结束detectionClusters = [detectionClusters;sensorDets];% #好< AGROW >结束结束%更新追踪是否有新的检测如果任何(isValidTime)如果isa(传感器{1},“drivingRadarDataGenerator”传感器)vehicleLength = {1} .Profiles.Length;其他的vehicleLength = {1} .ActorProfiles.Length传感器;结束confirmedTracks = updateTracks(跟踪、detectionClusters、时间);%更新鸟瞰的阴谋updateBEP (cep、egoCar detectionClusters、confirmedTracks positionSelector, velocitySelector);结束%拍图文档当汽车通过自我如果助教(1).Position (1) > 0 & & toSnap toSnap = false;snapnow结束结束

函数过滤器= initSimDemoFilter(检测)%使用二维恒定速度模型初始化一个trackingKF过滤器。%的状态向量(x, vx; y; v)%检测测量向量[x, y, vx; v)%结果,测量模型是H = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1)H = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1);过滤器= trackingKF (“MotionModel”,“二维恒定速度”,…“状态”,H * detection.Measurement…“MeasurementModel”H,…“StateCovariance”,H *检测。MeasurementNoise * H,…“MeasurementNoise”,detection.MeasurementNoise);结束

函数cep = createDemoDisplay (egoCar、传感器)%崭露头角hFigure =图(“位置”,(0,0,1200,640),“名字”,的传感器融合与合成数据的例子);movegui (hFigure [0, 1]);%移动左边的图,从上往下一点%添加一个汽车遵循自我车辆从背后的阴谋hCarViewPanel = uipanel (hFigure,“位置”[0 0 0.5 - 0.5),“标题”,“追逐相机视图”);hCarPlot =轴(hCarViewPanel);chasePlot (egoCar“父”,hCarPlot);%添加一个汽车的阴谋,从顶视图遵循自我的工具hTopViewPanel = uipanel (hFigure,“位置”[0 0.5 0.5 0.5],“标题”,顶视图的);hCarPlot =轴(hTopViewPanel);chasePlot (egoCar“父”hCarPlot,“ViewHeight”,130,“ViewLocation”[0 0],“ViewPitch”,90);%添加面板一个鸟瞰的阴谋hBEVPanel = uipanel (hFigure,“位置”(0.5 0 0.5 - 1),“标题”,“鸟”眼中的阴谋”);%为自我创造鸟瞰的阴谋车辆和传感器覆盖hBEVPlot =轴(hBEVPanel);frontBackLim = 60;cep = birdsEyePlot (“父”hBEVPlot,“Xlimits”,(-frontBackLim frontBackLim),“Ylimits”35 [-35]);%绘制雷达的覆盖区域为我= 1:6帽= coverageAreaPlotter (cep,“FaceColor”,“红色”,“EdgeColor”,“红色”);如果isa(传感器{我},“drivingRadarDataGenerator”)plotCoverageArea(帽、传感器{我}.MountingLocation (1:2),…{我}.RangeLimits传感器(2)、传感器{我}.MountingAngles(1),{我}.FieldOfView传感器(1));其他的plotCoverageArea({我}.SensorLocation帽,传感器,…传感器{我}。{我}MaxRange,传感器。{我}.FieldOfView偏航,传感器(1));结束结束%绘制视觉传感器的覆盖区域为我= = coverageAreaPlotter七8帽(cep),“FaceColor”,“蓝”,“EdgeColor”,“蓝”);如果isa(传感器{我},“drivingRadarDataGenerator”)plotCoverageArea(帽、传感器{我}.MountingLocation (1:2),…{我}.RangeLimits传感器(2)、传感器{我}.MountingAngles (1), 45);其他的plotCoverageArea({我}.SensorLocation帽,传感器,…传感器{我}。{我}MaxRange,传感器。偏航、45);结束结束%创建一个视觉检测绘图机把它放在一个结构体,以供将来使用detectionPlotter (cep“DisplayName的”,“愿景”,“MarkerEdgeColor”,“蓝”,“标记”,“^”);%将所有雷达探测到一个条目并将其存储为以后更新detectionPlotter (cep“DisplayName的”,“雷达”,“MarkerEdgeColor”,“红色”);%添加道路边界的阴谋laneMarkingPlotter (cep“DisplayName的”,“车道标记”);%将歌曲添加到鸟瞰的阴谋。显示最后10跟踪更新。trackPlotter (cep“DisplayName的”,“跟踪”,“HistoryDepth”10);轴(BEP.Parent“平等”);xlim (cep)。家长,[-frontBackLim frontBackLim]);ylim (cep)。家长,40 [-40]);%为地面实况添加大纲绘图仪outlinePlotter (cep“标签”,“地面实况”);结束

函数updateBEP (cep egoCar,检测、confirmedTracks psel, vsel)%更新道路边界及其显示(lmv, lmf) = laneMarkingVertices (egoCar);plotLaneMarking (findPlotter (cep),“DisplayName的”,“车道标记”)、lmv lmf);%更新地面实况数据(位置、偏航、长度、宽度、originOffset color] = targetOutlines (egoCar);plotOutline (findPlotter (cep),“标签”,“地面实况”)、位置、偏航、长度、宽度、“OriginOffset”originOffset,“颜色”、颜色);%更新障碍数据[bPosition, bYaw bLength、bWidth bOriginOffset, bColor, numBarrierSegments] = targetOutlines (egoCar,“障碍”);plotBarrierOutline (findPlotter (cep),“标签”,“地面实况”)、numBarrierSegments bPosition、bYaw bLength, bWidth,…“OriginOffset”bOriginOffset,“颜色”,bColor);%准备和更新检测显示N =元素个数(检测);detPos = 0 (N, 2);isRadar = true (N, 1);为i = 1: N detPos(我:)=检测{我}.Measurement (1:2)”;如果检测我{}。SensorIndex > 6%的视觉检测isRadar (i) = false;结束结束plotDetection (findPlotter (cep),“DisplayName的”,“愿景”),detPos (~ isRadar,:));plotDetection (findPlotter (cep),“DisplayName的”,“雷达”),detPos (isRadar:));%删除所有对象追踪不明的视觉检测%发电机之前更新跟踪显示。这些有ObjectClassID%参数值为0,包括对象等障碍。isNotBarrier = arrayfun (@ (t) t.ObjectClassID confirmedTracks) > 0;confirmedTracks = confirmedTracks (isNotBarrier);%准备和更新跟踪显示trackIDs = {confirmedTracks.TrackID};标签= cellfun (@num2str trackIDs,“UniformOutput”、假);[tracksPos, tracksCov] = getTrackPositions (confirmedTracks psel);tracksVel = getTrackVelocities (confirmedTracks vsel);plotTrack (findPlotter (cep),“DisplayName的”,“跟踪”)、tracksPos tracksVel tracksCov,标签);结束