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

打开脚本

这个例子展示了如何通过融合视觉和雷达传感器的数据来执行前向碰撞警告，以跟踪车辆前方的物体。

概述

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

为了本例的目的，一辆测试车(自我车辆)配备了各种传感器，并记录了它们的输出。本例中使用的传感器是:

视觉传感器，它提供了观察对象的分类和车道边界信息的列表。对象列表每秒报告10次。车道边界每秒被报告20次。
具有中、远程模式的雷达传感器，提供未分类的观测对象列表。对象列表每秒报告20次。
IMU，它每秒报告20次自我车辆的速度和转弯率。
摄像机，它记录了汽车前面的场景的视频剪辑。注意:此视频不供跟踪器使用，仅用于在视频中显示跟踪结果以供验证。

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

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

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

本例中的可视化是使用monoCamera而且birdsEyePlot．为了简单起见，创建和更新显示的函数被移到本例之外的辅助函数中。有关如何使用这些显示器的更多信息，请参见在车辆坐标中使用检测注释视频而且可视化传感器覆盖，检测和跟踪．

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

设置显示[videereader, videoDisplayHandle, bepploters, 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;time = time + timeStep;处理传感器检测作为跟踪器的objectDetection输入。[detections, laneborders, egoLane] = processDetections(.．.visionObjects (currentStep), radarObjects (currentStep),.．.inertialMeasurementUnit (currentStep), laneReports (currentStep),.．.egoLane、时间);使用objectDetections列表，返回更新到时间的轨道confirmedTracks = updateTracks(跟踪器，检测，时间);找到最重要的物体，计算正向碰撞。%的警告mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector);更新视频和鸟瞰图显示帧= readFrame (videoReader);%读取视频帧helperUpdateFCWDemoDisplay(框架、videoDisplayHandle bepPlotters,.．.lanebounds，传感器，confirmedTracks, mostImportantObject, positionSelector，.．.velocitySelector, visionObjects (currentStep), radarObjects (currentStep));%捕获快照如果time >= snapTime && time < snapTime + timeStep snapnow;结束结束

创建多对象跟踪器

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

的setupTracker函数的作用是:返回multiObjectTracker．当创建一个multiObjectTracker，考虑以下因素:

FilterInitializationFcn:可能的运动和测量模型。在这种情况下，物体被期望有恒定的加速度运动。尽管你可以为这个模型配置一个线性卡尔曼滤波器，initConstantAccelerationFilter配置扩展卡尔曼滤波器。参见“定义卡尔曼滤波器”一节。
AssignmentThreshold:探测距离轨道的距离有多远。该参数的默认值为30。如果有检测没有分配给轨道，但应该分配给轨道，则增加该值。如果有检测被分配到太远的轨道，则减小此值。这个例子使用了35。
DeletionThreshold:当一个音轨被确认时，它不应该在没有被分配检测的第一次更新时删除。相反，它应该滑行(预测)，直到它清楚地没有得到任何传感器信息来更新它。逻辑是，如果没有追踪到P的问乘以它应该被删除。该参数的默认值是5 / 5。在本例中，跟踪器每秒被调用20次，并且有两个传感器，因此不需要修改默认值。
ConfirmationThreshold:轨迹确认参数。每个未分配的检测都会初始化一个新的跟踪。其中一些检测可能是错误的，因此所有的轨道都初始化为“初步”．要确认踪迹，至少要被探测到米次N跟踪更新。的选择米而且N取决于对象的可见性。本例使用默认的3次更新中的2次检测。

的输出setupTracker是:

跟踪器- - -multiObjectTracker这是为本例配置的。
positionSelector-一个指定状态向量的哪个元素是位置的矩阵:position = positionSelector *状态
velocitySelector-一个矩阵，指定状态向量的哪个元素是速度:velocity = velocitySelector *状态

函数[tracker, positionSelector, velocitySelector] = setupTracker().．.“FilterInitializationFcn”@initConstantAccelerationFilter,.．.“AssignmentThreshold”, 35岁,“ConfirmationThreshold”3 [2],.．.“DeletionThreshold”5);%状态向量为:%在恒定速度下:状态= [x;vx;y;vy]%在恒定加速度下:状态= [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 0] *状态positionSelector = [1 0 0 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 0 1 0];结束

定义一个卡尔曼滤波器

的multiObjectTracker上一节定义的过滤器初始化函数使用本节定义的过滤器初始化函数创建卡尔曼过滤器(线性、扩展或无气味)。这个过滤器随后被用来跟踪自我载体周围的每个物体。

函数过滤器= initConstantAccelerationFilter(检测)这个函数演示了如何配置恒定加速度过滤器。的%输入是一个objectDetection，输出是一个跟踪过滤器。为了清晰起见，这个函数展示了如何配置trackingKF，% trackingEKF，或trackingUKF表示恒定加速度。％创建过滤器的步骤:% 1。定义运动模型和状态% 2。定义过程噪声% 3。定义度量模型% 4。根据测量值初始化状态向量% 5。根据测量噪声初始化状态协方差% 6。创建正确的过滤器步骤1:定义运动模型和状态本例使用恒定加速度模型，因此:月31 = @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²;Q = blkdiag(Q1d, Q1d);2-D过程噪声步骤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(detect . measurementnoise (1:2,1:2)， L, detect . measurementnoise (3:4,3:4)， L);步骤6:创建正确的过滤器。trackingKF使用'KF'， trackingEKF使用'EKF'， trackingUKF使用'UKF'FilterType =“算法”；%正在创建过滤器:开关FilterType情况下“算法”(STF, MF, state，.．.“StateCovariance”stateCov,.．.“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),.．.“StateTransitionJacobianFcn”STFJ,.．.“MeasurementJacobianFcn”MJF,.．.“ProcessNoise”,问.．.）;情况下“UKF”(STF, MF, state，.．.“StateCovariance”stateCov,.．.“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),.．.“α”1 e 1,.．.“ProcessNoise”,问.．.）;情况下KF的恒定加速度模型是线性的，可以使用KF定义测量模型:measurement = H * state%在这种情况下:测量% = [x, vx; y v] = H * [x, vx;斧子;y v,唉)%因此，H = [1 0 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;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0];过滤器= trackingKF (“MotionModel”，“二维恒定加速度”，.．.“MeasurementModel”H,“状态”、州、.．.“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),.．.“StateCovariance”, stateCov);结束结束

处理和格式化检测

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

过滤掉不必要的雷达杂波检测。雷达报告了许多与固定物体相对应的物体，包括:护栏、道路中间、交通标志等。如果在跟踪中使用这些检测，它们会在道路边缘产生固定物体的错误轨迹，因此必须在调用跟踪器之前将其删除。雷达目标被认为是非杂波，如果他们或静止在汽车前面或移动在其附近。
将检测格式化为跟踪器的输入，即objectDetection元素。看到processVideo而且processRadar本例末尾的支持函数。

函数[detections,laneBoundaries, egoLane] = processDetections ..．.(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)%的输入:% visionFrame -视觉传感器为这个时间帧报告的对象% radarFrame—雷达传感器在此时间段报告的对象IMUFrame -此时间段的惯性测量单元数据lane报告此时间框架自我线-估计的自我线% time -与时间框架相对应的时间清除杂波雷达物体[lane bounds, 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
时间—当前场景时间。

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

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

最重要的物体(MIO)被定义为在自我车道上且距离赛车最近的轨道，也就是正面最小的轨道x价值。为了降低误报的概率，只考虑已确认的轨迹。

一旦找到MIO，就计算汽车和MIO之间的相对速度。相对距离和相对速度决定了正向碰撞预警。FCW有三种情况:

安全(绿色):自我车道上没有汽车(没有MIO)， MIO正在远离汽车，或者与MIO的距离保持不变。
注意(黄色):MIO正在靠近汽车，但仍在FCW距离以上的距离。FCW距离使用Euro NCAP AEB测试协议计算。注意，这个距离随MIO和汽车之间的相对速度而变化，当关闭速度越快时，这个距离就越大。
警告(红色):MIO正在向汽车靠近，它的距离小于FCW距离， $美元d_{结合}$$ ．

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

$美元d_{结合}= 1.2 * v_ {rel} + \压裂{v_ {rel} ^ 2}{2现代{马克斯}}$$

地点:

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

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

美元现代{马克斯}$ 是最大减速，定义为重力加速度的40%。

函数mostImportantObject = findMostImportantObject (confirmedTracks egoLane、positionSelector velocitySelector)初始化输出和参数绪= [];缺省情况下，没有MIOtrackID = [];缺省情况下，没有与MIO关联的trackID结合= 3;默认情况下，如果没有MIO，那么FCW是“安全的”threatColor =“绿色”；%缺省情况下，威胁颜色为绿色maxX = 1000;足够远，以至于没有轨道会超过这个距离。gAccel = 9.8;恒定重力加速度，单位为m/s^2max减速= 0.4 * gAccel;%欧洲NCAP AEB定义滞后时间= 1.2;驾驶员在开始刹车前的延迟时间，单位为秒locations = gettracklocations (confirmedTracks, positionSelector);velocity = gettrackvelocity (confirmedTracks, velocitySelector);为i = 1:numel(confirmedTracks) x = locations (i,1);y =位置(我,2);relSpeed =速度(我,1);车道上两辆车之间的相对速度。如果x < maxX && x > 0%否则没有点检查yleftLane = polyval (egoLane。离开时,x);yrightLane = polyval (egoLane。右,x);如果(yrightLane <= y) && (y <= yleftLane) maxX = x;trackID =我;绪= confirmedTracks .TrackID;如果relSpeed < 0%相对速度表示物体正在接近计算预期制动距离欧洲NCAP AEB测试协议d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / max减速;如果x < = d% '警告'结合= 1;threatColor =“红色”；其他的%的谨慎结合= 2;threatColor =“黄色”；结束结束结束结束结束mostImportantObject =结构(“ObjectID”绪,“TrackIndex”trackID,“警告”结合,“ThreatColor”, threatColor);结束

总结

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

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

支持功能

readSensorRecordingsFile从文件中读取记录的传感器数据

函数[visionObjects, radarObjects, inertialMeasurementUnit, laneReports，.．.timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)%读取传感器记录|ReadDetectionsFile|函数读取记录的传感器数据文件。记录的数据是单个结构，被划分为%以下结构:％% # |inertialMeasurementUnit|，一个带有字段:timeStamp的结构数组，% velocity和yawRate。数组中的每个元素都对应于a%不同的步伐。% # |laneReports|，一个包含字段:左和右的结构数组。每个元素%对应于不同的时间步。左右都是带字段的结构:isValid, confidence，边界类型、偏移量、头部角度和曲率。% # |radarObjects|，一个带字段的结构数组:timeStamp(见下面)，% numObjects (integer)和object (struct)。数组的每个元素%对应不同的时间步。% |对象|是一个结构数组，其中每个元素是一个单独的对象，%与字段:id，状态，位置(x;y;z)，速度(vx,vy,vz)，%振幅，和rangeMode。注:z总是常数，vz=0。% # |visionObjects|，一个带字段的结构数组:timeStamp(见下)，% numObjects (integer)和object (struct)。数组的每个元素%对应不同的时间步。% |对象|是一个结构数组，其中每个元素是一个单独的对象，和字段:id，分类，位置(x;y;z)，%的速度(vx; v; vz),大小(dx, dy, dz)。注意:z = v = vz = dx = dz = 0％所记录的视觉和雷达对象的时间戳是uint64变量%自Unix时代以来的微秒。时间戳记录大约% 50毫秒间隔。两者之间是完全同步的视觉和雷达探测的记录，因此时间戳%不用于进一步的计算。=负载(sensorRecordingFileName);visionObjects = A.vision;radarObjects = A.radar;laneReports = A.lane;inertialMeasurementUnit = A.inertialMeasurementUnit;步伐= 0.05;%数据每50毫秒提供一次numSteps =元素个数(visionObjects);%记录的时间步数结束

processLanes将传感器报告的车道转换为parabolicLaneBoundary车道和保持一个持久的自我车道估计

函数[laneborders, egoLane] = processLanes(laneReports, egoLane)赛道边界根据来自记录的赛道报告更新。由于一些laneReports包含无效(isValid = false)报告或%不可能的参数值(-1e9)，这些车道报告被忽略和%使用以前的车道边界。leftLane = laneReports.left;rightLane = laneReports.right;检查报告的左车道的有效性。气孔导度= (leftLane。isValid && leftLane.confidence) &&.．.~ (leftLane。headingAngle == -1e9 || leftLane。曲率= = 1 e9);如果气孔导度egoLane。左= ([leftLane。曲率,leftLane。headingAngle, leftLane.offset],“双”）;结束更新左车道边界参数或使用以前的参数leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary (leftParams);leftBoundaries。力量= 1;检查报告的右车道的有效性。气孔导度= (rightLane。isValid && rightLane.confidence) &&.．.~ (rightLane。headingAngle == -1e9 || rightLane。曲率= = 1 e9);如果气孔导度egoLane。正确的= ([rightLane。曲率,rightLane。headingAngle, rightLane.offset],“双”）;结束更新右车道边界参数或使用以前的参数rightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary (rightParams);rightBoundaries。力量= 1;laneborders = [leftborders, rightborders];结束

findNonClutterRadarObjects去除被认为是杂波一部分的雷达物体

函数realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneborders)雷达物体包括许多属于杂波的物体。。杂波的定义是一个静止的物体不在前面。%的车。以下类型的对象可以作为非杂波对象:％车前任何物体汽车周围感兴趣区域内的任何移动物体，包括在汽车周围以横向速度移动的物体。%分配内存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));lanebounariesatobject = compute边界模型(laneborders, 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 =联盟(.．.cross (find(normv > minV)， find(inZone == 1))，.．.找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

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

函数(Vx, v) = calculateGroundSpeed (Vxi、Vyi egoSpeed)%的输入% (Vxi,Vyi):物体相对速度% egoSpeed:自我车辆速度%输出% [Vx,Vy]:地面物体速度Vx = Vxi + egoSpeed;计算纵向地面速度θ=量化(Vyi Vxi);%计算航向角Vy = Vx * tan();计算横向地面速度结束

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

函数postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)将视频对象处理为objectDetection对象numRadarObjects =元素个数(postProcessedDetections);numVisionObjects = visionFrame.numObjects;如果numVisionObjects class = class(visionFrame.object(1).position);visionMeasCov = cast(diag([2,2,100])， classToUse);%过程远景对象:为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(postProcessedDetections, realRadarObjects, t)将雷达对象处理为objectDetection对象numRadarObjects =元素个数(realRadarObjects);如果numRadarObjects classToUse = class(realRadarObjects(1).position);radarMeasCov = cast(diag([2,2,100])， classToUse);%处理雷达物体:为i=1:numRadarObjects对象= realRadarObjects(i);postProcessedDetections{我}= objectDetection (t).．.[object.position (1);object.velocity (1);object.position (2);object.velocity (2)),.．.“SensorIndex”2,“MeasurementNoise”radarMeasCov,.．.“MeasurementParameters”{2},.．.“ObjectAttributes”,{对象。id、对象。状态对象。振幅,object.rangeMode});结束结束结束

fcwmeas在这个向前碰撞警告示例中使用的测量函数

函数measurement = fcwmeas(state, sensorID)示例测量取决于传感器类型，该类型由%对象检测的测量参数属性。以下%使用两个sensorID值:% sensorID=1:视频对象，测量值为[x;vx;y]。% sensorID=2:雷达对象，测量值为[x;vx;y;vy]。%状态为:%匀速状态= [x;vx;y;vy]%恒定转动状态= [x;vx;y;vy;omega]%恒加速度状态= [x;vx;ax;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 = fcwmeasjac(state, sensorID)示例测量取决于传感器类型，该类型由%对象检测的测量参数属性。我们选择% sensorID=1表示视频对象，sensorID=2表示雷达对象。的使用以下两个sensorID值:% sensorID=1:视频对象，测量值为[x;vx;y]。% sensorID=2:雷达对象，测量值为[x;vx;y;vy]。%状态为:%匀速状态= [x;vx;y;vy]%恒定转动状态= [x;vx;y;vy;omega]%恒加速度状态= [x;vx;ax;y;vy;ay]numStates =元素个数(状态);雅可比矩阵= 0 (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;结束结束结束