%设置显示[videoReader, videoDisplayHandle, bepplotter, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4'，“SensorConfigurationData.mat”)；%读取记录的检测文件[视觉对象、雷达对象、惯性测量单位、laneReports、，．..timestep，numsteps] = readsensorRecordingsfile（'01_city_c2s_fcw_10s_sensor.mat')；计算初始自助泳道。如果记录的车道信息是无效，将车道边界定义为半车道的直线%汽车两侧的距离laneWidth=3.6；%米egoLane =结构(“左”，[0平面宽度/2]，“对”， [0 0 -laneWidth/2]);%准备一些时间变量时间= 0;%记录开始后的时间currentStep = 0;%当前时间步snapTime=9.3；%捕获显示器快照的时间初始化跟踪器[tracker, positionSelector, velocitySelector] = setupTracker();虽然当前步骤%更新场景计数器currentStep = currentStep + 1;time = time + timeStep;%处理传感器检测作为objectDetection输入到跟踪器[detections，laneBoundaries，egoLane]=处理检测(．..VisionObjects（CurrentStep），RadarObjects（CurrentStep），．..inertialMeasurementUnit (currentStep), laneReports (currentStep),．..egoLane、时间);%使用objectDetections列表，返回更新到时间的轨道确认技术= UpdateTracks（跟踪器，检测，时间）;找到最重要的对象并计算前向碰撞%警告mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector);%更新视频和鸟瞰图显示帧= readFrame (videoReader);%读取视频帧Helperupdatefcwdemodisplay（框架，videodisplayhandle，bepplotters，．..车道边界，传感器，确认轨道，最重要的对象，位置选择器，．..速度选择器、Vision对象（当前步骤）、雷达对象（当前步骤）；%捕获快照如果时间> = snaptime＆time 结束结束

作用[跟踪器，位置选择，VelocitySelector] = Setuptracker（）跟踪器= MultiObjectRacker（．..“FilterInitializationFcn”，@initConstantAccelerionFilter，．..“AssignmentThreshold”, 35岁,“ConfirmationThreshold”, [2 3],．..“删除阈值”5);状态向量为:%在恒定速度下：状态=[x；vx；y；vy]State = [x;vx;ax;y;vy;ay]%定义状态的哪一部分是位置。例如：% In constant velocity: [x;y] = [1 0 0 0;0 0 1 0恒加速度:[x;y] = [1 0 0 0 0 0;0 0 0 1 0 0positionSelector = [1 0 0 0 0;0 0 0 1 0 0];定义状态的哪一部分是速度。例如:恒定速度％：[x; y] = [0 1 0 0;0 0 0 1] *状态恒加速度:[x;y] = [0 1 0 0 0 0;0 0 0 1 0] * StateVelocitySelector = [0 1 0 0 0 0;0 0 0 0 1 0];结束

作用过滤器= initConstantAccelerationFilter(检测)%此函数显示如何配置恒定加速度过滤器％输入是ObjectDetection，输出是跟踪滤波器。%为清楚起见，此函数显示了如何配置trackingKF，％trackingekf，或trackingukf用于恒定加速度。％%创建过滤器的步骤:%1.定义运动模型和状态％2.定义过程噪声% 3。定义测量模型%4.根据测量值初始化状态向量％5.根据测量噪声初始化状态协方差%6.创建正确的过滤器%步骤1：定义运动模型和状态%本例使用恒定加速度模型，因此：stf = @constacc;%状态转移函数，用于EKF和UKFSTFJ = @constaccjac;%状态转移函数雅可比矩阵，仅适用于EKF%运动模型表示状态为[x；vx；ax；y；vy；ay]你也可以使用constvel和constveljac来设置一个常量%速度模型、constturn和constturnjac，用于设置恒定转弯％速率模型，或写自己的模型。步骤2:定义过程噪声dt = 0.05;%已知时间步长大小σ= 1;%未知加速度变化率的幅度沿一维的过程噪声Q1D = [DT ^ 4/4，DT ^ 3/2，DT ^ 2/2;DT ^ 3/2，dt ^ 2，dt;dt ^ 2/2，dt，1] * sigma ^ 2;Q = Blkdiag（Q1D，Q1D）;%二维过程噪声步骤3:定义度量模型MF = @fcwmeas;%测量功能，用于EKF和UKFmjf = @fcwmeasjac;%测量雅可比函数，仅用于EKF%步骤4:基于测量初始化一个状态向量传感器测量[x;vx;y;vy]和恒定加速度模型%状态是[x；vx；ax；y；vy；ay]，因此状态向量初始化为零。状态= [detection.Measurement (1);detection.Measurement (2);0;detection.Measurement (3);detection.Measurement (4);0);%步骤5:基于测量初始化状态协方差%的噪音。状态中没有直接测量的部分是%指定一个较大的测量噪声值来说明这一点。L = 100;%相对于测量噪声而言，数值很大stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2)， L, detection.MeasurementNoise(3:4,3:4)， L);步骤6:创建正确的过滤器。％使用'kf'trackingkf，'ekf'for trackingekf，或towninggukf的'ukf'FilterType =“算法”；%创建过滤器：转换FilterType案件“算法”过滤器=跟踪EKF（STF、MF、状态、，．..“状态协方差”stateCov,．..“MeasurementNoise”，检测..easurementnoise（1：4,1：4），．..“StateTransitionJacobianFcn”，STFJ，．..“MeasurementJacobianFcn”MJF,．..“ProcessNoise”,问．..)；案件“UKF”filter = trackingUKF(STF, MF, state，．..“状态协方差”stateCov,．..“MeasurementNoise”，检测..easurementnoise（1：4,1：4），．..“阿尔法”1 e 1,．..“ProcessNoise”,问．..)；案件KF的％丹尾匹配模型是线性的，可以使用KF%定义测量模型:测量= H *状态％ 在这种情况下：%测量=[x；vx；y；vy]=H*[x；vx；ax；y；vy；ay]％SO，H = [1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0]％方法自动计算ProcessNoise％丹卡卡塞运动模型H=[1 0 0 0；0 1 0 0 0；0 0 0 1 0；0 0 0 0 0 1 0]；过滤器=跟踪KF(“MotionModel”，“2D恒定加速度”，．..“测量模型”H,“国家”、州、．..“MeasurementNoise”，检测..easurementnoise（1：4,1：4），．..“状态协方差”, stateCov);结束结束

作用[detections,laneBoundaries, egoLane] = processDetections．..(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)%投入：% visionFrame -视觉传感器在此时间范围内报告的对象%雷达帧-雷达传感器在此时间帧报告的目标％imuframe  - 此时间帧的惯性测量单元数据% lane frame -此时间段的lane报告%egoLane-估计的egoLane%时间-对应于时间范围的时间清除雷达目标的杂波[laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);realRadarObjects = findNonClutterRadarObjects (radarFrame.object,．..雷达帧.numObjects、IMUFrame.velocity、laneBoundaries）；%如果未报告任何对象，则返回空列表%统计对象总数检测= {};如果(visionFrame。numObjects + numel(realRadarObjects)) == 0回来；结束%处理剩余的雷达目标detections = processRadar(detections, realRadarObjects, time);%处理视频对象detections = processVideo(detections, visionFrame, time);结束

作用mostImportantObject=FindMOSTMONTANTOBJECT（确认跟踪、电子车道、位置选择器、速度选择器）%初始化输出和参数绪= [];%默认情况下，没有MIOtrackID = [];%默认情况下，没有与MIO关联的trackID结合= 3;%默认情况下，如果没有MIO，那么FCW是“安全的”威胁色=“绿色”；%默认情况下，威胁颜色为绿色maxx = 1000;向前走得足够远，以至于没有轨道会超过这个距离。gaccel = 9.8;%恒定重力加速度，单位为m/s^2max减速= 0.4 * gAccel;％欧元ncap aeb定义延迟时间=1.2；%开始制动前驾驶员的延迟时间（秒）position = getTrackPositions(confirmedTracks, positionSelector);velocity = gettrackvelocity (confirmedTracks, velocitySelector);为i = 1:numel(confirmedTracks) x = positions(i,1);y =位置(我,2);relSpeed =速度(我,1);沿着车道，两车之间的相对速度。如果x < maxX && x >%否则就没有意义了yleftLane = polyval (egoLane。离开时,x);yrightLane = polyval (egoLane。右,x);如果（Yrightlane <= Y）&&（Y <= YLEFTLANE）maxx = x;trackid = i;mio = confirmedtracks（i）.trackid;如果relSpeed < 0%相对速度表示物体越来越近%计算预期制动距离根据％欧洲NCAP AEB测试协议d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / max减速;如果x < = d％'警告'FCW=1；threatColor=“红色”；其他的％'警告'FCW=2；threatColor=“黄色”；结束结束结束结束结束mostimportObject = struct（“ObjectID”，mio，“TrackIndex”trackID,“警告”，fcw，“威胁色”, threatColor);结束

作用[视觉对象、雷达对象、惯性测量单位、laneReports、，．..timestep，numsteps] = readsensorRecordingsfile（SensorRecordingFilename）读取传感器记录％| readdetectionsfile |功能读取录制的传感器数据文件。记录的数据是一个单独的结构，它被分为百分比以下子结构：％% # |inertialMeasurementUnit|，一个带有字段的结构数组:timeStamp，%速度和yawRate。数组中的每个元素都对应于a%不同的步伐。%#| laneReports |，一个包含字段的结构数组：左和右。每个元素%数组的长度对应于不同的时间步。% left和right都是带字段的结构:isValid, confidence，%边界类型、偏移、头角和曲率。% # |radarObjects|，一个包含以下字段的结构数组:％numObjects（整数）和对象（结构）。数组的每个元素%对应于不同的时间步。|是一个结构数组，其中每个元素是一个单独的对象，% with fields: id, status, position(x;y;z)， velocity(vx,vy,vz)，%振幅和rangeMode。%注：z始终为常数，且vz=0。%#| visionObjects |，一个带有字段的结构数组：timeStamp（见下文），％numObjects（整数）和对象（结构）。数组的每个元素%对应于不同的时间步。|是一个结构数组，其中每个元素是一个单独的对象，%带字段：id、分类、位置（x；y；z），%的速度(vx; v; vz),大小(dx, dy, dz)。注意:z = v = vz = dx = dz = 0％录制视觉和雷达对象的时间戳是uint64变量%自Unix纪元以来保持微秒。时间戳记录在％50毫秒分开。之间存在完整的同步%视觉和雷达探测记录，因此时间戳%未在进一步计算中使用。a = load（sensorrecordingfilename）;VisionObjects = A.Vision;radarobjects = a.radar;lanereports = a.lane;inertialmeasurementunit = a.inertialmeasurementunit;Timestep = 0.05;每50毫秒提供％数据numSteps =元素个数(visionObjects);%记录的时间步数结束

作用[LaneBondaries，eGrane]=进程通道（laneReports，eGrane）%车道边界将根据录制的车道报告进行更新。由于某些lanereports包含无效（ISValid = FALSE）报告或％不可能的参数值（-1e9），这些车道报告被忽略了%使用上一条车道边界。Leftlane = LaneReports.left;Rightlane = LaneReports.Right;%检查报告的左车道的有效性气孔导度= (leftLane。isValid && leftLane.confidence) &&．..~（leftLane.headingAngle==-1e9 | | leftLane.curvature==-1e9）；如果气孔导度egoLane。左= ([leftLane。曲率,leftLane。headingAngle, leftLane.offset],“双”)；结束%更新左车道边界参数或使用以前的参数leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary (leftParams);leftBoundaries。力量= 1;检查报告的右车道是否有效气孔导度= (rightLane。isValid && rightLane.confidence) &&．..~ (rightLane。方向角== -1e9 ||右曲率= = 1 e9);如果cond egoLane.right=投射（[rightLane.curvature，rightLane.headingAngle，rightLane.offset]，“双”)；结束%更新右车道边界参数或使用以前的参数rightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary (rightParams);rightBoundaries。力量= 1;[左边界，右边界];结束

作用realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneBoundaries)％雷达对象包括属于杂波的许多对象。的定义为不在前面的静止物体%的车。以下类型的对象作为非杂波传递:％汽车前面的任何物体% #任何在汽车周围感兴趣区域的移动物体，包括在汽车周围以横向速度移动的物体％分配内存normVs = 0 (numRadarObjects, 1);inLane = 0 (numRadarObjects, 1);inZone = 0 (numRadarObjects, 1);%参数LaneWidth=3.6；被认为在汽车前面的东西ZoneWidth = 1.7 *巷宽;更广泛的兴趣领域minv = 1;任何移动速度低于minV的物体都被认为是静止的为j = 1:numRadarObjects [vx, vy] = calculateGroundSpeed(radarObject(j).velocity(1)，radarObject(j).velocity(2)，egoSpeed);normVs规范(j) = ((vx, v));laneBoundariesAtObject = computeBoundaryModel(laneBoundaries, radarObject(j).position(1));laneCenter =意味着(laneBoundariesAtObject);inLane(j) = (abs(radarObject(j).position(2) - laneCenter) <= LaneWidth/2);inZone(j) = (abs(radarObject(j).position(2) - laneCenter) <= max(abs(vy)*2, ZoneWidth));结束realRadarObjectsIdx =联盟(．..intersect(find(normVs > minV)， find(inZone == 1))，．..找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

作用(Vx, v) = calculateGroundSpeed (Vxi、Vyi egoSpeed)%的输入% (Vxi,Vyi):物体相对速度自我速度:自我车辆速度%输出%:地面物体速度Vx=Vxi+速度；％计算纵向地速度Theta = atan2（vyi，vxi）;计算航向角vy = vx * tan（theta）;%计算横向地面速度结束

作用postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)％将视频对象处理到ObjectDetection对象中numRadarObjects=numel（后处理检测）；numVisionObjects=visionFrame.numObjects；如果numVisionObjects classToUse = class(visionFrame.object(1).position);visionMeasCov = cast(diag([2,2,2,100])， classToUse);%过程视觉对象:为i=1:numVisionObjects对象= visionFrame.object(i);postProcessedDetections {numRadarObjects + i} = objectDetection (t)．..[对象位置（1）；对象速度（1）；对象位置（2）；0]，．..'sensorindex', 1“MeasurementNoise”visionMeasCov,．..“MeasurementParameters”{1},．..“ObjectClassID”object.classification,．..“ObjectAttributes”,{对象。id, object.size});结束结束结束

作用PostProcessedDetections = ProcessRadar（后处理Ddetections，RealRadarobjects，T）％将雷达对象处理到ObjectDetection对象中numRadarObjects =元素个数(realRadarObjects);如果numRadarObjects classToUse=class（realRadarObjects（1）.位置）；radarMeasCov=cast（diag（[2,2,2100]），classToUse）；％过程雷达对象：为i=1:numRadarObjects对象= realRadarObjects(i);postProcessedDetections{我}= objectDetection (t)．..[object.position (1);object.velocity (1);object.position (2);object.velocity (2)),．..'sensorindex'2，“MeasurementNoise”，拉达梅阿斯科夫，．..“MeasurementParameters”{2},．..“ObjectAttributes”，{object.id，object.status，object.amplitude，Object.RangeMode}）;结束结束结束

作用测量= fcwmeas(state, sensorID)％测量值取决于传感器类型，这些传感器类型% objectDetection的MeasurementParameters属性。以下％两个SensorID值使用：%sensorID=1：视频对象，测量值为[x；vx；y]。%sensorID=2：雷达目标，测量值为[x；vx；y；vy]。%状态为:恒速状态= [x;vx;y;vy][x;vx;y;vy;% Constant acceleration state = [x;vx;ax;y;如果元素个数(状态)< 6恒定转弯或恒定速度转换传感器案件1%的视频测量= [状态（1：3）;0);案件2%的雷达测量=状态（1：4）;结束其他的%恒加速度转换传感器案件1%的视频测量=[状态(1:2);国家(4);0);案件2%的雷达测量=[状态（1:2）；状态（4:5）]；结束结束结束

作用fcwmeasjac(state, sensorID)％测量值取决于传感器类型，这些传感器类型% objectDetection的MeasurementParameters属性。我们选择% sensorID=1表示视频对象，% sensorID=2表示雷达对象。的%以下两个sensorID值被使用:%sensorID=1：视频对象，测量值为[x；vx；y]。%sensorID=2：雷达目标，测量值为[x；vx；y；vy]。%状态为:恒速状态= [x;vx;y;vy][x;vx;y;vy;% Constant acceleration state = [x;vx;ax;y;numStates =元素个数(状态);= 0 (4, numStates，'喜欢'， 状态）;如果元素个数(状态)< 6恒定转弯或恒定速度转换传感器案件1%的视频雅可比（1,1）=1；雅可比（2,2）=1；雅可比（3,3）=1；案件2%的雷达雅可比（1,1）=1；雅可比（2,2）=1；雅可比（3,3）=1；雅可比矩阵(4,4)= 1;结束其他的%恒加速度转换传感器案件1%的视频雅可比矩阵(1,- 1)= 1;雅可比矩阵(2,2)= 1;雅可比矩阵(3、4)= 1;案件2%的雷达雅可比（1,1）=1；雅可比（2,2）=1；雅可比（3,4）=1；雅可比（4,5）=1；结束结束结束