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

开放脚本

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

概述

前向碰撞预警(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”）;％读取记录的检测文件[视觉对象、雷达对象、惯性测量单位、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 = currentStep + 1;time = time + timeStep;%处理传感器检测作为objectDetection输入到跟踪器[detections, laneBoundaries, 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,．..laneBoundaries，传感器，confirmedTracks，mostImportantObject，positionSelector，．..速度选择器、Vision对象（当前步骤）、雷达对象（当前步骤）；%捕获快照如果time >= snapTime && time < snapTime + timeStep snapnow;结尾结尾

创建多目标跟踪器

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

的setupTracker函数的作用是:返回multiObjectTracker.在创建multiObjectTracker，考虑以下：

FilterInitializationFcn:可能的运动和测量模型。在这种情况下，物体会有恒定的加速度运动。虽然你可以为这个模型配置一个线性卡尔曼滤波器，初始化加速滤波器配置扩展卡尔曼滤波器。参见“定义卡尔曼滤波器”部分。
AssignmentThreshold:探测距离轨道有多远。该参数的默认值为30。如果有未分配到轨迹的检测(但应该分配到)，则增加该值。如果有检测被分配到太远的轨道，减少这个值。本例使用35。
DeletionThreshold：当确认轨道时，不应在第一次更新时将其删除，因为没有为其分配任何检测。相反，应使其滑行（预测），直到确定轨道没有获得任何传感器信息来更新它。逻辑是如果轨道丢失P从…里面问应该删除的次数。该参数的默认值为5-out- 5。在这种情况下，跟踪器每秒被调用20次，而且有两个传感器，所以不需要修改默认值。
ConfirmationThreshold:轨道确认参数。每个未分配的检测都初始化一个新轨道。其中一些检测可能是假的，所以所有的轨迹都被初始化为“初步”.要确认轨迹，必须至少检测到该轨迹米次N跟踪更新。的选择米和N取决于物体的可见度。本例使用默认的3次更新中的2次检测。

输出setupTracker是:

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

函数[tracker, positionSelector, velocitySelector] = setupTracker() tracker = multiObjectTracker()．..“FilterInitializationFcn”@initConstantAccelerationFilter,．..'AssignmentThreshold', 35岁,“ConfirmationThreshold”3 [2],．..“删除阈值”5);状态向量为:%在恒定速度下：状态=[x；vx；y；vy]％在恒定加速度：状态= [X; VX;斧; 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 0] *州positionSelector = [1 0 0 0 0;0 0 0 1 0 0];定义状态的哪一部分是速度。例如:在恒定速度:[x;y] = [0 1 0 0;0 0 0 1] * State恒加速度:[x;y] = [0 1 0 0 0 0;0 0 0 1 0] * StatevelocitySelector = [0 1 0 0 0;0 0 0 1 0];结尾

定义一个卡尔曼滤波器

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

函数滤波器= initConstantAccelerationFilter（检测）%此函数显示如何配置恒定加速度过滤器% input是一个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设置恒定转弯% rate模型，或编写自己的模型。％步骤2：定义过程噪声dt = 0.05;%已知的时间步长σ= 1;未知的加速度变化率的幅度％％沿着一个维度的过程噪声q = [dt^4/4, dt^3/2, dt^2/2;dt ^ 3/2, dt ^ 2, dt;Dt ^2/2, Dt, 1] * ^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;％A大相对于测量噪声数stateCov = blkdiag（detection.MeasurementNoise（1：2,1：2），L，detection.MeasurementNoise（3：4,3：4），L）;％6步：创建正确的过滤器。%使用'KF'表示trackingKF， 'EKF'表示trackingEKF，或'UKF'表示trackingUKFFilterType =“算法”；%创建过滤器:开关过滤式情况下“算法”filter = trackingEKF(STF, MF, state，．..“StateCovariance”，stateCov，．..'MeasurementNoise'detection.MeasurementNoise (1:4, 1:4),．..“StateTransitionJacobianFcn”STFJ,．..“MeasurementJacobianFcn”MJF,．..'ProcessNoise'，Q．..）;情况下“UKF”滤波器= trackingUKF（STF，MF，状态，．..“StateCovariance”，stateCov，．..'MeasurementNoise'detection.MeasurementNoise (1:4, 1:4),．..“α”1 e 1,．..'ProcessNoise'，Q．..）;情况下'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 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'，“二维恒定加速度”，．..“MeasurementModel”， H，“国家”、州、．..'MeasurementNoise'detection.MeasurementNoise (1:4, 1:4),．..“StateCovariance”, stateCov);结尾结尾

处理和格式化检测项

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

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

函数[detections,laneBoundaries, egoLane] = processDetections．..(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)%投入：% visionFrame -视觉传感器在此时间范围内报告的对象％radarFrame  - 对象由为此时间帧中的雷达传感器报告% IMUFrame -此时间帧的惯性测量单元数据％laneFrame  - 这个时间框架车道报告%自我-估计的自我巷% time—时间范围所对应的时间％拆下雷达杂波的对象[laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);realRadarObjects = findNonClutterRadarObjects (radarFrame.object,．..雷达帧.numObjects、IMUFrame.velocity、laneBoundaries）；%如果未报告任何对象，则返回空列表%统计对象总数检测= {};如果(visionFrame。numObjects + numel(realRadarObjects)) == 0返回；结尾%处理剩余的雷达目标检测= processRadar（检测，realRadarObjects，时间）;%处理视频对象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_{结合}= 1.2 * v_ {rel} + \压裂{v_ {rel} ^ 2}{2现代{马克斯}}$$

在哪里：

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

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

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

函数mostImportantObject=FindMOSTMONTANTOBJECT（确认跟踪、电子车道、位置选择器、速度选择器）%初始化输出和参数绪= [];%默认情况下，没有MIOtrackID = [];%默认情况下，没有与MIO关联的trackID结合= 3;％在默认情况下，如果没有MIO，然后FCW是“安全”threatColor ='绿色'；％默认情况下，威胁颜色为绿色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  0%不作任何检查yleftLane = polyval（egoLane.left，X）;yrightLane = polyval（egoLane.right，X）;如果(yrightLane <= y) && (y <= yleftLane) maxX = x;trackID =我;绪= confirmedTracks .TrackID;如果relSpeed < 0%相对速度表示物体正在靠近%计算预期制动距离根据%欧洲NCAP AEB测试协议d = ABS（relSpeed）* delayTime + relSpeed ^ 2/2 / maxDeceleration;如果x < = d% '警告'结合= 1;threatColor =“红色”；别的%的谨慎结合= 2;threatColor =“黄色”；结尾结尾结尾结尾结尾mostImportantObject =结构(“ObjectID”绪,'TrackIndex'trackID,“警告”结合,“威胁色”，threatColor）;结尾

总结

这个例子说明了如何为配备视觉，雷达和传感器IMU车辆建立前方碰撞预警系统。它使用objectDetection对象将传感器报告传递给multiObjectTracker物体融合了它们并在自我车前追踪物体。

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

金宝app支持功能

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

函数[视觉对象、雷达对象、惯性测量单位、laneReports、，．..timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)％读感器录音% |ReadDetectionsFile|函数用于读取记录的传感器数据文件。％所记录的数据是分割成一个单一的结构%以下结构:％% # |inertialMeasurementUnit|，一个带有字段的结构数组:timeStamp，%速度和yawRate。数组中的每个元素都对应于a%不同的步伐。%#| laneReports |，一个包含字段的结构数组：左和右。每个元素数组的%对应不同的时间步长。% left和right都是带字段的结构:isValid, confidence，%边界类型、偏移量、航向角和曲率。％＃| radarObjects |，一个结构阵列字段：时间标记（见下文），% numObjects (integer)和对象(struct)。数组中的每个元素%对应于不同的时间步。|是一个结构数组，其中每个元素是一个单独的对象，％与字段：ID，状态，位置（X; Y; Z），速度（VX，VY，VZ），％的振幅，和rangeMode。%注意:z总是常数，vz=0。%#| visionObjects |，一个带有字段的结构数组：timeStamp（见下文），% numObjects (integer)和对象(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车道和保持一个持续的自我车道估计

函数[LaneBondaries，eGrane]=进程通道（laneReports，eGrane）% Lane边界将根据录音中的lanreports进行更新。由于一些laneReports包含无效(isValid = false)报告或%不可能的参数值(-1e9)，这些通道报告被忽略%使用上一条车道边界。leftLane = laneReports.left;rightLane = laneReports.right;%检查报告的左车道的有效性COND =（leftLane.isValid && leftLane.confidence）&&．..~ (leftLane。方向角== -1e9 ||左车道曲率= = 1 e9);如果气孔导度egoLane。左= ([leftLane。曲率,leftLane。headingAngle, leftLane.offset],'双倍的'）;结尾%更新左车道边界参数或使用以前的参数leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary（leftParams）;leftBoundaries.Strength = 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;[左边界，右边界];结尾

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

函数realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneBoundaries)雷达目标包括许多属于杂波的目标。％杂波被定义为静止的对象不是在前面%的车。以下类型的对象作为非杂波传递:％汽车前面的任何物体％＃在围绕汽车感兴趣的区域的任何移动物体，包括％反对这一举动在围绕汽车的横向速度%分配内存normVs =零（numRadarObjects，1）;inLane =零（numRadarObjects，1）;银座=零（numRadarObjects，1）;%参数LaneWidth=3.6；％什么是车前考虑ZoneWidth = 1.7 * LaneWidth;更广泛的兴趣领域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 + egoSpeed;计算纵向地面速度θ=量化(Vyi Vxi);％计算方位角Vy = Vx * tan();计算侧向地面速度结尾

processVideo皈依报道视觉对象的列表objectDetection对象

函数postProcessedDetections = processVideo（postProcessedDetections，visionFrame，t）的%将视频对象处理为对象检测对象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});结尾结尾结尾

处理雷达将报告的雷达对象转换为objectDetection对象

函数postProcessedDetections = processRadar(postProcessedDetections, realRadarObjects, t)%将雷达对象处理为对象检测对象numRadarObjects =元素个数(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);object.velocity (2)),．..“SensorIndex”2,'MeasurementNoise'radarMeasCov,．..'MeasurementParameters'，{2}，．..“ObjectAttributes”,{对象。id、对象。状态对象。振幅,object.rangeMode});结尾结尾结尾

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

函数测量= 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;如果numel（状态）<6恒定转弯或恒定速度开关sensorID情况下1%的视频测量=[状态(1:3);0);情况下2％雷达测量=状态(1:4);结尾别的%恒定加速度开关sensorID情况下1%的视频测量= [状态（1：2）;状态（4）;0);情况下2％雷达测量=[状态（1:2）；状态（4:5）]；结尾结尾结尾

fcwmeasjac在这个前向碰撞预警例子中使用的测量函数的雅可比矩阵

函数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，“喜欢”、州);如果numel（状态）<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；结尾结尾结尾