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

打开脚本

这个例子展示了如何执行向前碰撞警告,从视觉和雷达传感器融合数据跟踪对象在汽车前面。

概述

向前碰撞警告(结合)是一个重要的特性在驾驶员辅助和自动驾驶系统,我们的目标是提供正确、及时、可靠的警告司机即将发生碰撞前与前面的车辆。为了实现目标,车辆配备前置愿景和雷达传感器。传感器融合需要准确的概率增加警告和最小化的概率错误的警告。

对于本示例中,一个测试车(自我车辆)配备各种传感器及其输出记录。本例中使用的传感器有:

视觉传感器,提供观察对象列表的分类和车道边界的信息。对象列表报告每秒10倍。车道边界被报道每秒20倍。
雷达传感器介质和远程模式,提供非保密观察对象的列表。对象列表报告每秒20倍。
IMU,报道的速度和周转周期自我车辆每秒20倍。
摄像机,记录现场的视频剪辑在汽车的前面。注意:这个视频跟踪器使用的不是,只会显示跟踪结果对视频验证。

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

从传感器获得的数据。
传感器数据融合得到一个跟踪列表,即。,estimated positions and velocities of the objects in front of the car.
基于轨道和结合标准发出警告。结合标准是基于欧元NCAP AEB测试过程和考虑物体的相对距离和相对速度在汽车的前面。

关于跟踪多个对象的更多信息,请参阅多个对象跟踪。

本例中使用的可视化monoCamera和birdsEyePlot。为简便起见,创建和更新显示的功能被转移到这个例子以外的辅助函数。有关如何使用这些显示的更多信息,见注释视频使用检测车辆坐标和可视化传感器覆盖范围、检测和跟踪。

这个例子是一个脚本,与这里显示的主体和形式的辅助例程在接下来的章节里,本地函数。关于当地的更多细节功能,看到功能添加到脚本。

%设置显示[videoReader, videoDisplayHandle bepPlotters、传感器)= helperCreateFCWDemoDisplay (“01 _city_c2s_fcw_10s.mp4”,“SensorConfigurationData.mat”);%读取记录检测文件[visionObjects, radarObjects inertialMeasurementUnit laneReports,…步伐,numSteps] = readSensorRecordingsFile (“01 _city_c2s_fcw_10s_sensor.mat”);%计算初始自我巷。如果车道信息记录%无效,车道边界定义为直线半个车道%距离两边的车巷宽= 3.6;%米egoLane =结构(“左”(0 0巷宽/ 2),“对”,0 0巷宽/ 2);%准备一些时间变量时间= 0;%的开始记录以来currentStep = 0;%当前步伐snapTime = 9.3;%的时间捕获的快照显示%初始化跟踪器(跟踪、positionSelector velocitySelector] = setupTracker ();而currentStep < numSteps & & ishghandle (videoDisplayHandle)%更新场景计数器currentStep = currentStep + 1;时间=时间+步伐;%处理传感器检测objectDetection输入追踪(检测、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,…velocitySelector, visionObjects (currentStep), radarObjects (currentStep));%捕获快照如果时间> = snapTime & & < snapTime +步伐snapnow;结束结束

创建多目标跟踪器

的multiObjectTracker跟踪对象在自我车辆基于对象列表报告的愿景和雷达传感器。通过融合两种传感器的信息,虚假的碰撞警告的概率会降低。

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

FilterInitializationFcn:可能的运动和测量模型。在这种情况下,预计将有一个恒定的加速度运动对象。虽然您可以配置一个线性卡尔曼滤波模型,initConstantAccelerationFilter配置一个扩展的卡尔曼滤波器。请参见定义一个卡尔曼滤波器的部分。
AssignmentThreshold:检测能从多远的踪迹。这个参数的默认值是30。如果有检测不分配给追踪,但应该增加这个值。如果有检测,分配给轨道太远,减少该值。这个示例使用35。
DeletionThreshold:当一个跟踪被证实,它不应该被删除第一个更新,没有检测是分配给它。相反,它应该摸样(预测),直到很明显,跟踪是得不到任何传感器信息来更新它。的逻辑是,如果错过了P的问时间应该被删除。这个参数的默认值是5个中的5个。在这种情况下,跟踪器被称为20次,有两个传感器,所以没有需要修改默认。
ConfirmationThreshold:参数确认跟踪。初始化一个新的追踪每一次未赋值的检测。其中的一些检测可能是假的,所以所有的痕迹都初始化为“初步”。确认轨道,它必须至少被探测到米次N跟踪更新。的选择米和N取决于对象的可见性。下面的例子使用了默认2检测的3的更新。

的输出setupTracker是:

跟踪器- - -multiObjectTracker对于这种情况配置。
positionSelector——一个矩阵指定哪些元素的状态向量的位置:位置= positionSelector *状态
velocitySelector——一个矩阵指定哪些元素状态向量的速度:速度= velocitySelector *状态

函数(跟踪、positionSelector velocitySelector] = setupTracker()追踪= multiObjectTracker (…“FilterInitializationFcn”@initConstantAccelerationFilter,…“AssignmentThreshold”,35岁,“ConfirmationThreshold”3 [2],…“DeletionThreshold”5);%的状态向量为:%在恒定速度:状态= [x, vx; y; v]%在恒定加速度:状态= [x, vx;斧子;y v; 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 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);结束

定义一个卡尔曼滤波器

的multiObjectTracker定义在前一节中使用过滤器的初始化函数定义在这一节中创建一个卡尔曼滤波器(线性、扩展或无味)。这个过滤器是用于跟踪每个对象的自我。

函数过滤器= initConstantAccelerationFilter(检测)%这个函数展示了如何配置一个恒定加速度过滤器。的%的输入是一个objectDetection和输出跟踪滤波器。%为清晰起见,这个函数展示了如何配置一个trackingKF,% trackingEKF或trackingUKF恒定加速度。%%的步骤创建一个过滤器:% 1。定义运动模型和状态% 2。定义流程的噪音% 3。定义测量模型% 4。初始化状态向量的基础上测量% 5。基于测量噪声的初始化状态协方差% 6。创建正确的过滤器%步骤1:定义运动模型和状态%这个示例使用一个常数加速度模型,所以:月31 = @constacc;%状态转换函数,EKF与UKFSTFJ = @constaccjac;%状态转换函数雅可比矩阵,只对卡尔曼滤波器%运动模型意味着状态[x, vx;斧子;y v,唉)%还可以使用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] *σ^ 2;Q = blkdiag (Q1d Q1d);% 2 d过程噪声%步骤3:定义度量模型MF = @fcwmeas;%的测量功能,EKF与UKFMJF = @fcwmeasjac;%测量雅可比矩阵函数,只有卡尔曼滤波器%步骤4:初始化一个基于测量的状态向量%传感器测量[x, vx; y; v]和恒定加速度模型%状态[x, vx;斧子;y v,唉),所以第三和第六的元素%状态向量初始化为零。状态= [detection.Measurement (1);detection.Measurement (2);0;detection.Measurement (3);detection.Measurement (4);0);%第五步:根据测量初始化状态协方差%的噪音。的部分国家,并不是直接测量%分配大的测量噪声值占。L = 100;%大量相对于测量噪声stateCov = blkdiag (detection.MeasurementNoise (1:2, 1:2), L, detection.MeasurementNoise (3:4, 3:4), L);%第六步:创建正确的过滤器。%为trackingKF使用的KF, trackingEKF的EKF、UKF的trackingUKFFilterType =“算法”;%创建过滤器:开关FilterType情况下“算法”过滤器= trackingEKF(底座、MF、状态,…“StateCovariance”stateCov,…“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),…“StateTransitionJacobianFcn”STFJ,…“MeasurementJacobianFcn”MJF,…“ProcessNoise”,问…);情况下“UKF”过滤器= trackingUKF(底座、MF、状态,…“StateCovariance”stateCov,…“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),…“α”1 e 1,…“ProcessNoise”,问…);情况下KF的% ConstantAcceleration模型可以使用线性和KF%定义度量模型:测量= H *状态%在这种情况下:测量% = [x, vx; y v] = H * [x, vx;斧子;y v,唉)%,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是自动计算的% ConstantAcceleration运动模型H = [1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1);过滤器= trackingKF (“MotionModel”,“二维恒定加速度”,…“MeasurementModel”H,“状态”、州、…“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),…“StateCovariance”,stateCov);结束结束

检测过程和格式

记录信息之前必须处理和格式化的跟踪器可以使用它。这有以下步骤:

过滤掉不必要的雷达杂波信号检测。雷达报告许多对象对应于固定的对象,其中包括:护栏、道路中间,交通标志,等等。如果这些检测是用于跟踪,他们创建虚假跟踪固定对象边缘的道路,因此在调用跟踪器之前必须清除。雷达对象被认为是nonclutter如果他们要么是固定在汽车前面或在其附近移动。
格式化的跟踪检测作为输入,即。一个数组的objectDetection元素。看到processVideo和processRadar金宝app在这个例子中支持功能。

函数(检测、laneBoundaries egoLane] = processDetections…(radarFrame visionFrame IMUFrame、laneFrame egoLane,时间)%的输入:% visionFrame——对象视觉传感器对这个报告的时间框架% radarFrame -对象报告的雷达传感器的时间框架% IMUFrame—惯性测量单元数据时间框架% laneFrame——巷报告这个时间框架% egoLane——估计自我巷%的时间——时间对应的时间框架%去除杂波雷达对象[laneBoundaries, egoLane] = processLanes (laneFrame egoLane);realRadarObjects = findNonClutterRadarObjects (radarFrame.object,…radarFrame。numObjects IMUFrame。速度,laneBoundaries);%如果没有报道对象返回一个空列表%计算对象的总数检测= {};如果(visionFrame。numObjects +元素个数(realRadarObjects)) = = 0返回;结束%处理剩余的雷达对象检测= processRadar(检测、realRadarObjects、时间);%过程视频对象检测= processVideo(检测、visionFrame、时间);结束

更新追踪

更新追踪,调用updateTracks方法用下面的输入:

跟踪器- - -multiObjectTracker配置。请参见“创建的多目标跟踪器”部分。
检测——列表objectDetection是由的对象processDetections
时间——当前的场景。

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

找到最重要的对象和问题提出碰撞警告

最重要的对象(绪)被定义为自我的轨道巷和最近的汽车前,即。,最小的积极x价值。降低误报的可能性,只有确认追踪。

一旦发现绪,汽车和绪之间的相对速度计算。的相对距离和相对速度确定碰撞警告。有3例的结合:

安全的(绿色):没有汽车自我巷(没有MIO),绪远离汽车,或MIO)之间的距离保持不变。
谨慎(黄色):绪正在接近,但仍在远处结合以上距离。结合距离计算使用欧元NCAP AEB测试协议。注意,这个距离随绪之间的相对速度,汽车,和关闭速度较高时更大。
警告(红色):绪靠近汽车,和它的距离小于结合距离, $美元d_{结合}$$ 。

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

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

地点:

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

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

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

函数mostImportantObject = findMostImportantObject (confirmedTracks egoLane、positionSelector velocitySelector)%初始化输出和参数绪= [];%在默认情况下,没有绪trackID = [];%在默认情况下,没有相关trackID绪结合= 3;%在默认情况下,如果没有绪,然后结合“安全”threatColor =“绿色”;%在默认情况下,威胁的颜色是绿色的maxX = 1000;%足够远向前,不预计将超过这个距离跟踪gAccel = 9.8;%不变的重力加速度,在m / s ^ 2maxDeceleration = 0.4 * gAccel;%欧洲五星AEB定义滞后时间= 1.2;%为司机开始前制动延迟时间,以秒为单位位置= getTrackPositions (confirmedTracks positionSelector);速度= getTrackVelocities (confirmedTracks velocitySelector);为i = 1:元素个数(confirmedTracks) x =位置(我,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%相对速度表明对象是越来越近了%计算制动距离根据预期%欧洲五星AEB测试协议d = abs (relSpeed) *时延+ relSpeed ^ 2 / 2 / maxDeceleration;如果x < = d% '警告'结合= 1;threatColor =“红色”;其他的%的谨慎结合= 2;threatColor =“黄色”;结束结束结束结束结束mostImportantObject =结构(“ObjectID”绪,“TrackIndex”trackID,“警告”结合,“ThreatColor”,threatColor);结束

总结

这个例子展示了如何创建一个向前碰撞预警系统为车辆配备视觉,雷达和IMU传感器。它使用objectDetection对象通过传感器报告multiObjectTracker对象,和跟踪融合对象前的自我。

尝试使用不同的参数跟踪器,看看它们是如何影响跟踪质量。试着修改跟踪滤波器使用trackingKF或trackingUKF,或者定义一个不同的运动模式,例如,恒定速度或常数。最后,您可以定义自己的运动模型。

金宝app支持功能

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

函数[visionObjects, radarObjects inertialMeasurementUnit laneReports,…步伐,numSteps] = readSensorRecordingsFile (sensorRecordingFileName)%读取传感器记录%的| ReadDetectionsFile |函数读取传感器数据记录文件。%的记录数据是单一结构分为%以下结构:%% # | inertialMeasurementUnit |,一个结构体数组字段:时间戳,%的速度,和yawRate。数组的每个元素对应一个%不同的步伐。% # | laneReports |,一个结构体数组字段:左和右。每个元素%的数组对应一个不同的步伐。%左右都是结构字段:isValid,信心,% boundaryType,抵消、headingAngle和曲率。% # | radarObjects |,一个结构体数组字段:时间戳(见下文),% numObjects(整数)和客体(结构)。数组的每个元素%对应一个不同的步伐。% | |对象是一个结构体数组,每个元素都是一个独立的对象,%的领域:id、状态、位置(x, y, z)、速度(v, vx 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毫秒。有一个完整的之间的同步%的录音愿景和雷达探测,因此时间戳%用于进一步的计算。=负载(sensorRecordingFileName);visionObjects = A.vision;radarObjects = A.radar;laneReports = A.lane;inertialMeasurementUnit = A.inertialMeasurementUnit;步伐= 0.05;%提供数据每50毫秒numSteps =元素个数(visionObjects);%的数量记录的步伐结束

processLanessensor-reported车道转换为parabolicLaneBoundary车道和维护一个持久自我巷估计

函数[laneBoundaries, egoLane] = processLanes (laneReports egoLane)%车道边界更新基于laneReports录音。%因为一些laneReports包含无效(isValid = false)或报告%不可能的参数值(1 e9),这些车道被忽略和报告使用%前面的车道边界。leftLane = laneReports.left;rightLane = laneReports.right;%检查报告左车道上的有效性气孔导度= (leftLane。& & & & leftLane.confidence是否是可用的)…~ (leftLane。headingAngle = = 1 e9 | | leftLane。曲率= = 1 e9);如果气孔导度egoLane。左= ([leftLane。曲率,leftLane。headingAngle, leftLane.offset],“双”);结束%更新左边的车道边界参数或使用之前的leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary (leftParams);leftBoundaries。力量= 1;%检查报告的有效性右车道气孔导度= (rightLane。& & & & rightLane.confidence是否是可用的)…~ (rightLane。headingAngle = = 1 e9 | | rightLane。曲率= = 1 e9);如果气孔导度egoLane。正确的= ([rightLane。曲率,rightLane。headingAngle, rightLane.offset],“双”);结束%更新右车道边界参数或使用之前的rightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary (rightParams);rightBoundaries。力量= 1;laneBoundaries = [leftBoundaries, rightBoundaries];结束

findNonClutterRadarObjects去除雷达的对象被认为是杂乱的一部分

函数realRadarObjects = findNonClutterRadarObjects (radarObject numRadarObjects、egoSpeed laneBoundaries)%的雷达对象包括许多对象,属于杂乱。%杂物被定义为一个固定的对象不是前面的%的车。以下类型的对象通过nonclutter:%% #任何对象前面的车% #任何移动物体在汽车周围的感兴趣的领域,包括%的物体移动速度横向汽车%分配内存normVs = 0 (numRadarObjects, 1);inLane = 0 (numRadarObjects, 1);inZone = 0 (numRadarObjects, 1);%的参数巷宽= 3.6;%是什么考虑前面的车ZoneWidth = 1.7 *巷宽;%更感兴趣的领域minV = 1;%的任何对象,低于minV被认为是静止的为j = 1: numRadarObjects [vx, v] = 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) < =巷宽/ 2);inZone (j) = (abs (radarObject (j) .position (2) - laneCenter) < = max (abs (v) * 2, ZoneWidth));结束realRadarObjectsIdx =联盟(…相交(找到(normVs > minV),发现(inZone = = 1)),…找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

calculateGroundSpeed计算真实的地面速度radar-reported对象的相对速度和自我车辆速度

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

processVideo将视觉对象的列表objectDetection对象

函数postProcessedDetections = processVideo (postProcessedDetections visionFrame t)%过程为objectDetection对象的视频对象numRadarObjects =元素个数(postProcessedDetections);numVisionObjects = visionFrame.numObjects;如果numVisionObjects classToUse =类(visionFrame.object (1) .position);visionMeasCov =投(诊断接头([2,2,2100]),classToUse);%过程视觉对象:为i = 1: numVisionObjects对象= visionFrame.object(我);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 =类(realRadarObjects (1) .position);radarMeasCov =投(诊断接头([2,2,2100]),classToUse);%过程雷达对象:为i = 1: numRadarObjects对象= realRadarObjects(我);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测量这向前碰撞警告示例中使用的函数

函数sensorID测量= fcwmeas(状态)%的例子测量依赖于传感器类型、报告%的MeasurementParameters属性objectDetection。以下%使用两个sensorID值:% sensorID = 1:视频对象,测量[x, vx; y]。% sensorID = 2:雷达对象,测量[x, vx; y; v]。%的状态是:%恒定速度状态= [x, vx; y; v)%常数将状态= [x, vx; y, v,ω)%加速度恒定状态= [x, vx;斧子;y v,唉)如果元素个数(状态)< 6%不变转弯或恒定速度开关sensorID情况下1%的视频测量=[状态(1:3);0);情况下2%的雷达测量=状态(1:4);结束其他的%恒定加速度开关sensorID情况下1%的视频测量=[状态(1:2);国家(4);0);情况下2%的雷达测量=[状态(1:2);状态(4:5)];结束结束结束

fcwmeasjac雅可比矩阵的测量函数这向前碰撞警告示例中使用

函数雅可比矩阵= fcwmeasjac(状态,sensorID)%的例子测量依赖于传感器类型、报告%的MeasurementParameters属性objectDetection。我们选择视频对象和sensorID % sensorID = 1 = 2雷达对象。的%以下两个sensorID值使用:% sensorID = 1:视频对象,测量[x, vx; y]。% sensorID = 2:雷达对象,测量[x, vx; y; v]。%的状态是:%恒定速度状态= [x, vx; y; v)%常数将状态= [x, vx; y, v,ω)%加速度恒定状态= [x, vx;斧子;y v,唉)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;结束结束结束