使用传感器融合的前向碰撞预警

打开脚本

此示例显示了如何通过从视觉和雷达传感器融合数据来执行前进碰撞警告，以跟踪车辆前面的物体。

概述

前向碰撞预警(FCW)是驾驶员辅助和自动驾驶系统中的一个重要功能，其目标是在前方车辆即将发生碰撞前向驾驶员提供正确、及时和可靠的警告。为了实现这一目标，车辆配备了前方视觉和雷达传感器。为了提高准确预警的概率，减少错误预警的概率，需要进行传感器融合。

为了这个例子的目的，一辆测试车(ego车辆)配备了各种传感器，它们的输出被记录下来。本例中使用的传感器为:

视觉传感器，提供观察对象的列表及其分类和关于车道边界的信息。对象列表每秒报告10次。每秒钟报告20次车道边界。
雷达传感器具有中型和长距离模式，提供了未分类的观察对象的列表。每秒报告对象列表20次。
IMU，每秒报告ego车辆的速度和转弯速度20次。
摄像机，其中录制了一段视频剪辑的现场前的车。注:本视频不用于跟踪器，仅用于视频上显示跟踪结果以供验证。

提供正向碰撞警告的过程包括以下步骤：

从传感器获取数据。
融合传感器数据，得到一个轨道列表，即，估计的位置和速度的物体在汽车前面。
根据轨道和FCW标准发出警告。FCW标准基于欧洲NCAP AEB测试程序，并考虑到与前方物体的相对距离和相对速度。

有关跟踪多个对象的更多信息，请参见多个对象跟踪．

使用此示例中的可视化完成monoCamera和birdsEyePlot．对于简洁起见，创建和更新显示的功能被移动到此示例之外的帮助函数。有关如何使用这些显示的详细信息，请参阅使用车辆坐标中的检测注释视频和可视化传感器覆盖，检测和轨道．

这个例子是一个脚本，主体在这里显示，辅助例程以局部函数的形式出现在后面的小节中。有关本地函数的详细信息，请参见向脚本添加函数．

%设置显示[videoReader, videoDisplayHandle, bepplotter, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4'，“SensorConfigurationData.mat”）;%读取记录的检测文件[visionObjects, radarObjects, inertialMeasurementUnit, laneReports，......timestep，numsteps] = readsensorRecordingsfile（'01_city_c2s_fcw_10s_sensor.mat'）;计算初始自助泳道。如果记录的车道信息是无效，将车道边界定义为半车道的直线%汽车两侧的距离巷宽= 3.6;％仪表egoLane =结构(“左”， [0 0 laneWidth/2]，“对”， [0 0 -laneWidth/2]);%准备一些时间变量时间= 0;%记录开始后的时间currentStep = 0;%当前步伐snapTime = 9.3;%捕获显示快照的时间初始化跟踪器[tracker, positionSelector, velocitySelector] = setupTracker();而currentStep < numSteps && ishhandle (videoDisplayHandle)%更新场景计数器CurrentStep = CurrentStep + 1;时间=时间+时间戳;％处理传感器检测作为对象的输入到跟踪器[detections, laneBoundaries, egoLane] = processDetections(......VisionObjects（CurrentStep），RadarObjects（CurrentStep），......InertialMeasurementUnit（CurrentStep），LaneReports（CurrentStep），......Egolane，时间）;％使用ObjectDetections列表，返回曲目，更新时间确认技术= UpdateTracks（跟踪器，检测，时间）;找到最重要的对象并计算前向碰撞%的警告mostimportObject = findstommportantObject（确认有限公司，egolane，positeSelector，速度选择）;%更新视频和鸟瞰图显示框架= ReadFrame（Videoreader）;%读取视频帧Helperupdatefcwdemodisplay（框架，videodisplayhandle，bepplotters，......车道边界，传感器，确认轨道，最重要的对象，位置选择器，......velocitySelector, visionObjects (currentStep), radarObjects (currentStep));％捕获快照如果时间> = snaptime＆time 结束结束

创建多对象跟踪器

的MultiObjectTracker.根据视觉和雷达传感器报告的物体列表跟踪ego车辆周围的物体。通过融合两个传感器的信息，降低了错误碰撞预警的概率。

的setupTracker函数返回MultiObjectTracker.．当创建一个MultiObjectTracker.，考虑以下情况:

FilterInitializationFcn:可能的运动和测量模型。在这种情况下，物体会有恒定的加速度运动。虽然你可以为这个模型配置一个线性卡尔曼滤波器，initConstantAccelerationFilter配置扩展的卡尔曼滤波器。请参阅“定义卡尔曼筛选器”部分。
分配察觉:探测距离轨道有多远。该参数的默认值为30。如果有未分配到轨迹的检测(但应该分配到)，则增加该值。如果有检测被分配到太远的轨道，减少这个值。本例使用35。
deletionthreshold.:当一个轨道被确认时，它不应该在第一次更新时被删除，因为没有检测到它。相反，它应该滑行(预测)，直到很清楚，轨道没有得到任何传感器信息来更新它。逻辑是，如果错过了轨道P的问它应该被删除。此参数的默认值为5-OUT-5。在这种情况下，跟踪器称为秒的20次，并且有两个传感器，因此不需要修改默认值。
确认察觉:轨道确认参数。每个未分配的检测都初始化一个新轨道。其中一些检测可能是假的，所以所有的轨迹都被初始化为'暂定的'．要确认一个轨迹，至少要检测到它米次N跟踪更新。的选择米和N取决于物体的可见度。本例使用默认的3次更新中的2次检测。

的输出setupTracker是:

追踪器- 这MultiObjectTracker.配置为此案例。
位置选择-指定状态向量中哪些元素的位置的矩阵:位置= PositionSelector *状态
速度选择-指定状态向量中哪些元素是速度的矩阵:Velocity = VelocitySelector *状态

函数[跟踪器，位置选择，VelocitySelector] = Setuptracker（）跟踪器= MultiObjectRacker（......“FilterInitializationFcn”@initConstantAccelerationFilter,......“AssignmentThreshold”35，“ConfirmationThreshold”3 [2],......“DeletionThreshold”5);州矢量是：%匀速状态:State = [x;vx;y;vy]State = [x;vx;ax;y;vy;ay]定义位置在国家的哪个部分。例如:恒定速度的％：[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 0 1 0] *状态VelocitySelector = [0 1 0 0 0 0;0 0 0 0 1 0];结束

定义卡尔曼滤波器

的MultiObjectTracker.使用本节定义的滤波器初始化函数创建卡尔曼滤波器(线性、扩展或无迹)。然后，这个过滤器用于跟踪ego车辆周围的每个对象。

函数过滤器= 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 ='ekf'；%创建过滤器:开关FilterType案件'ekf'filter = trackingEKF(STF, MF, state，......“StateCovariance”stateCov,......“MeasurementNoise”，检测..easurementnoise（1：4,1：4），......“StateTransitionJacobianFcn”STFJ,......“MeasurementJacobianFcn”，mjf，......“ProcessNoise”,问......）;案件“UKF”filter = trackingUKF(STF, MF, state，......“StateCovariance”stateCov,......“MeasurementNoise”，检测..easurementnoise（1：4,1：4），......“α”，1e-1，......“ProcessNoise”,问......）;案件KF的％丹尾匹配模型是线性的，可以使用KF%定义测量模型:测量= H *状态％ 在这种情况下：测量% = [x, vx; y v] = H * [x, vx;斧子;y v,唉)％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 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 1 0];过滤器= trackingKF (“MotionModel”，“二维恒定加速度”，......“MeasurementModel”H,“状态”， 状态，......“MeasurementNoise”，检测..easurementnoise（1：4,1：4），......“StateCovariance”，Statecov）;结束结束

处理和格式化检测

记录的信息必须经过处理并格式化后才能被跟踪器使用。这有以下步骤:

过滤掉不必要的雷达杂波检测。雷达报告了许多与固定物体相对应的物体，包括:护栏、道路中间、交通标志等。如果在跟踪中使用这些检测，它们会在道路边缘产生固定物体的虚假轨迹，因此必须在调用跟踪器之前将其清除。雷达目标被认为是无杂波的，如果它们是静止在汽车前面或在其附近移动。
将检测格式化为跟踪器的输入，即数组objectDetection元素。看到processVideo和processRadar万博1manbetx在此示例结束时支持功能。

函数[检测，泳道，egolane] = ProcessDetections......(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)%的输入:% visionFrame -视觉传感器在此时间范围内报告的对象%雷达帧-雷达传感器在此时间帧报告的目标％imuframe  - 此时间帧的惯性测量单元数据% lane frame -此时间段的lane报告%自我-估计的自我巷% time—时间范围所对应的时间清除雷达目标的杂波[LaneBoundaries，egolane] = Processlanes（Laneframe，Egolane）;RealRadarobjects = findnonclutterradarobjects（RadarFrame.Object，......radarFrame。numObjects IMUFrame。速度,laneBoundaries);%如果没有报告对象，则返回一个空列表%统计对象总数检测= {};如果（VisionFrame.Numobjects + Numel（RealRadarobjects））== 0返回；结束％处理剩余的雷达物体detections = processRadar(detections, realRadarObjects, time);%处理视频对象detections = processVideo(detections, visionFrame, time);结束

更新追踪

要更新追踪器，请呼叫UpdateTracks.具有以下输入的方法：

追踪器- 这MultiObjectTracker.这是之前配置的。参见“创建多对象跟踪器”部分。
检测- 的清单objectDetection创建的对象processDetections
时间—当前场景时间。

跟踪器的输出是结构体数组的痕迹。

找到最重要的物体并发出前向碰撞警告

最重要的物体（MIO）被定义为自我车道中的轨道，最接近汽车前面，即，具有最小的正面x价值。为了降低误报的概率，只考虑已确认的轨迹。

一旦发现了MIO，就计算了汽车和MIO之间的相对速度。相对距离和相对速度决定了前向碰撞警告。FCW有3例：

安全（绿色）：自我泳道没有汽车（没有MIO），MIO远离汽车，或与MIO的距离保持恒定。
注意(黄色):MIO离车越来越近，但距离仍高于FCW距离。采用Euro NCAP AEB测试协议计算FCW距离。注意，这个距离随MIO和汽车之间的相对速度而变化，当接近速度更高时，距离更大。
警告(红色):MIO离车越来越近，距离小于FCW距离， $美元d_{结合}$$ ．

欧洲NCAP AEB测试协议定义以下距离计算：

$d_ {fcw} = 1.2 * v_ {rel} + \ frac {v_ {rel} ^ 2} {2a_ {max}}$

地点:

$美元d_{结合}$$ 为前方碰撞预警距离。

$v_ {rel}$ 是两辆车之间的相对速度。

$a_ {max}$ 为最大减速，定义为重力加速度的40%。

函数mostImportantObject = findMostImportantObject (confirmedTracks egoLane、positionSelector velocitySelector)％初始化输出和参数mio = [];%默认情况下，没有MIOtrackID = [];%默认情况下，没有与MIO关联的trackID结合= 3;%默认情况下，如果没有MIO，那么FCW是“安全的”threatColor =“绿色”；%默认情况下，威胁颜色为绿色maxx = 1000;向前走得足够远，以至于没有轨道会超过这个距离。gaccel = 9.8;％恒定重力加速，在m / s ^ 2中maxdeceleration = 0.4 * gaccel;％欧元ncap aeb定义滞后时间= 1.2;在开始制动之前，驱动程序的延迟时间为单位positions = getTrackpositions（ConfirmedTracks，PositeSelector）;速度= getTrackvelocities（确认有限公司，速度选择）;为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％'警告'结合= 1;threatColor =“红色”；其他的％'警告'结合= 2;threatColor ='黄色'；结束结束结束结束结束mostimportObject = struct（“ObjectID”，mio，“TrackIndex”，trackid，'警告'，fcw，'威胁color', threatColor);结束

总结

这个例子展示了如何为装有视觉、雷达和IMU传感器的车辆创建一个前向碰撞预警系统。它使用objectDetection对象将传感器报告传递给MultiObjectTracker.融合它们和跟踪自我车辆前面的物体的对象。

尝试使用跟踪器的不同参数来查看它们如何影响跟踪质量。尝试修改要使用的跟踪过滤器trackingKF或trackingUKF，或定义一种不同的运动模式，例如恒定速度或恒定转弯。最后，你可以尝试定义你自己的运动模型。

万博1manbetx支持功能

ReadSensorRecordingsfile.从文件中读取录制的传感器数据

函数[visionObjects, radarObjects, inertialMeasurementUnit, laneReports，......timestep，numsteps] = readsensorRecordingsfile（SensorRecordingFilename）读取传感器记录％| readdetectionsfile |功能读取录制的传感器数据文件。记录的数据是一个单独的结构，它被分为百分比以下子结构：％％＃| InertialMeasurementUnit |，带字段的结构数组：时间戳，%速度和yawRate。数组中的每个元素都对应于a%不同的步伐。% # |laneReports|，一个带有字段:left和right的结构数组。每个元素数组的%对应不同的时间步长。% left和right都是带字段的结构:isValid, confidence，%边界类型、偏移量、航向角和曲率。% # |radarObjects|，一个包含以下字段的结构数组:％numObjects（整数）和对象（结构）。数组的每个元素%对应不同的时间步长。|是一个结构数组，其中每个元素是一个单独的对象，% with fields: id, status, position(x;y;z)， velocity(vx,vy,vz)，%振幅和rangeMode。%注意:z总是常数，vz=0。% # |visionObjects|，一个带有字段的结构数组:％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);%记录的时间步数结束

Processlanes.转换传感器报告的车道parabolicLaneBoundary车道并保持持久的自我车道估计

函数[laneBoundaries, egoLane] = processLanes(laneReports, egoLane)% Lane边界将根据录音中的lanreports进行更新。由于某些lanereports包含无效（ISValid = FALSE）报告或％不可能的参数值（-1e9），这些车道报告被忽略了%使用以前的车道边界。Leftlane = LaneReports.left;Rightlane = LaneReports.Right;检查所报告的左车道是否有效气孔导度= (leftLane。isValid && leftLane.confidence) &&......~ (leftLane。方向角== -1e9 ||左车道曲率= = 1 e9);如果cond egolane.left = cast（[Leftlane.Curvature，Leftlane.Headingangangle，Leftlane.offset]，“双”）;结束％更新左侧车道边界参数或使用前一个leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary (leftParams);leftBoundaries。力量= 1;％检查报告的右车道的有效性气孔导度= (rightLane。isValid && rightLane.confidence) &&......〜（rightlane.headingangle == -1e9 || rightlane.curvature == -1e9）;如果气孔导度egoLane。正确的= ([rightLane。曲率,rightLane。headingAngle, rightLane.offset],“双”）;结束％更新正确的车道边界参数或使用前一个RightParams = Egolane.Right;右翼=抛物面=抛物线（RightParams）;RightBoundaries.strength = 1;LaneBoutnaries = [LeftBoundaries，RightBoundaries];结束

findNonClutterRadarObjects移除被认为是杂波一部分的雷达目标

函数realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneBoundaries)％雷达对象包括属于杂波的许多对象。的定义为不在前面的静止物体%的车。以下类型的对象作为非杂波传递:％％＃汽车前面的任何物体% #任何在汽车周围感兴趣区域的移动物体，包括在汽车周围以横向速度移动的物体％分配内存normVs = 0 (numRadarObjects, 1);inLane = 0 (numRadarObjects, 1);inZone = 0 (numRadarObjects, 1);%的参数巷宽= 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 = Union（......相交（查找（normvs> minv），查找（inzone == 1）），......找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

calculateGroundSpeed从相对速度和自我飞行器的速度计算雷达报告的目标的真实地面速度

函数[VX，VY] = CalculdCroundspeed（VXI，VYI，EGOSPEED）%的输入% (Vxi,Vyi):物体相对速度自我速度:自我车辆速度%输出％[VX，VY]：地面对象速度Vx = Vxi + egoSpeed;％计算纵向地速度Theta = atan2（vyi，vxi）;计算航向角vy = vx * tan（theta）;计算侧向地面速度结束

processVideo将报告的视觉对象转换为objectDetection对象

函数postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)％将视频对象处理到ObjectDetection对象中numRadarObjects =元素个数(postProcessedDetections);numVisionObjects = visionFrame.numObjects;如果numvisionObjects Classtouse = class（VisionFrame.Object（1）.position）;VisionMeascov =铸造（DIAG（[2,2,2,100]），Classe）;%过程视觉对象:为i = 1：numvisionObjects对象= VisionFrame.Object（i）;postprocessedDetections {numradarobjects + i} = ObjectDetection（t，......[object.position (1);object.velocity (1);object.position (2);0),......'sensorindex'，1，“MeasurementNoise”visionMeasCov,......“MeasurementParameters”{1},......“ObjectClassID”object.classification,......“ObjectAttributes”,{对象。id, object.size});结束结束结束

processRadar将报告的雷达对象转换为列表objectDetection对象

函数PostProcessedDetections = ProcessRadar（后处理Ddetections，RealRadarobjects，T）％将雷达对象处理到ObjectDetection对象中numradarobjects = numel（realradarobjects）;如果numRadarObjects classToUse = class(realRadarObjects(1).position);radarMeasCov = cast(diag([2,2,2,100])， classToUse);％过程雷达对象：为i=1:numRadarObjects对象= realRadarObjects(i);postProcessedDetections{我}= objectDetection (t)......[object.position (1);object.velocity (1);object.position (2);对象。velocity（2）]，......'sensorindex'2，“MeasurementNoise”radarMeasCov,......“MeasurementParameters”{2},......“ObjectAttributes”，{object.id，object.status，object.amplitude，Object.RangeMode}）;结束结束结束

fcwmeas该转发碰撞警告示例中使用的测量功能

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

fcwmeasjac在此前进碰撞警告示例中使用的测量函数的Jacobian

函数Jacobian = fcwmeasjac（州，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;％恒定加速状态= [x; vx;斧头; y; vy; ay]numstates = numel（州）;Jacobian = Zeros（4，Numstates）;如果元素个数(状态)< 6恒定转弯或恒定速度开关sensorID案件1％ 视频雅可比矩阵(1,- 1)= 1;雅可比矩阵(2,2)= 1;雅可比矩阵(3)= 1;案件2%的雷达雅可比矩阵(1,- 1)= 1;雅可比矩阵(2,2)= 1;雅可比矩阵(3)= 1;雅可比矩阵(4,4)= 1;结束其他的%恒定加速度开关sensorID案件1％ 视频雅可比矩阵(1,- 1)= 1;雅可比矩阵(2,2)= 1;雅可比矩阵(3、4)= 1;案件2%的雷达雅可比矩阵(1,- 1)= 1;雅可比矩阵(2,2)= 1;雅可比矩阵(3、4)= 1;雅可比矩阵(4、5)= 1;结束结束结束