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

打开脚本

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

概述

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

出于该示例的目的，测试汽车（EGO车辆）配备有各种传感器，并记录其输出。用于此示例的传感器是：

视觉传感器，提供观察对象的列表及其分类和关于车道边界的信息。对象列表每秒报告10次。每秒钟报告20次车道边界。
雷达传感器具有中型和长距离模式，提供了未经分类的观察对象的列表。对象列表每秒报告20次。
IMU报告了自我车辆的速度和转速每秒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 %更新场景计数器CurrentStep = CurrentStep + 1;时间=时间+时间戳;％处理传感器检测作为对象的输入到跟踪器[detections, laneBoundaries, egoLane] = processDetections(......visionObjects (currentStep), radarObjects (currentStep),......InertialMeasurementUnit（CurrentStep），LaneReports（CurrentStep），......埃尔兰，时间）;％使用ObjectDetections列表，返回曲目，更新时间确认技术= UpdateTracks（跟踪器，检测，时间）;找到最重要的对象并计算前向碰撞％ 警告mostimportObject = findmessimportortObject（确认有限公司，egolane，positeSelector，速度选择器）;%更新视频和鸟瞰图显示帧= ReadFrame（Videoreader）;％读取视频帧helperUpdateFCWDemoDisplay(框架、videoDisplayHandle bepPlotters,......车道边界，传感器，确认轨道，最重要的对象，位置选择器，......VelocitySelector，VisionObjects（CurrentStep），RadarObjects（CurrentStep））;％捕获快照如果time >= snapTime && time < snapTime + timeStep snapnow;结束结束

创建多对象跟踪器

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

的setupTracker函数的作用是:返回multiObjectTracker．创建A.multiObjectTracker，考虑以下情况:

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

输出setupTracker是:

跟踪器- - -multiObjectTracker配置为此案例。
位置选择-指定状态向量中哪些元素的位置的矩阵:position = positionSelector * State
velocitySelector-指定状态向量中哪些元素是速度的矩阵:Velocity = VelocitySelector *状态

函数[tracker, positionSelector, velocitySelector] = setupTracker() tracker = multiObjectTracker()......“FilterInitializationFcn”@initConstantAccelerationFilter,......“AssignmentThreshold”35，“ConfirmationThreshold”3 [2],......'deletionthreshold'5);州矢量是：恒定速度％：状态= [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] * State恒定加速度的％：[x; y] = [0 1 0 0 0 0;0 0 0 0 1 0] *状态velocitySelector = [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;%相对于测量噪声而言，数值很大stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2)， L, detection.MeasurementNoise(3:4,3:4)， L);步骤6:创建正确的过滤器。%使用'KF'表示trackingKF， 'EKF'表示trackingEKF，或'UKF'表示trackingUKFFilterType ='ekf'；%创建过滤器:开关FilterType情况下'ekf'filter = trackingEKF(STF, MF, state，......“StateCovariance”stateCov,......“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),......“StateTransitionJacobianFcn”STFJ,......“MeasurementJacobianFcn”，mjf，......“ProcessNoise”,问......)；情况下“UKF”filter = trackingUKF(STF, MF, state，......“StateCovariance”stateCov,......“MeasurementNoise”detection.MeasurementNoise (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 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];filter = trackingkf（“MotionModel”，“二维恒定加速度”，......“MeasurementModel”H,'状态'， 状态，......“MeasurementNoise”detection.MeasurementNoise (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，Emolane）;RealRadarObjects = findnonclutterradarobjects（RadarFrame.Object，......radarframe.numobjects，imuframe.velocity，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} $$ 是两辆车之间的相对速度。

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

函数mostimportantObject = findmessimportObject（确认有限公司，egolane，positeSelector，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;在开始制动之前，驱动程序的延迟时间％，以秒为单位位置= getTrackpositions（确认托架，位置选择）;速度= 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 =我;绪= confirmedTracks .TrackID;如果RELSPEED <0.%相对速度表示物体正在靠近%计算预期制动距离根据%欧洲NCAP AEB测试协议d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / max减速;如果x < = d% '警告'结合= 1;threatColor =“红色”；其他的%的谨慎fcw = 2;threatColor =“黄色”；结束结束结束结束结束mostImportantObject =结构('Objectid'绪,“TrackIndex”，trackid，“警告”结合,'威胁color', threatColor);结束

概括

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

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

万博1manbetx支持功能

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

函数[VisionObjects，Radarobjects，InertialMeasurementUnit，LaneReports，......timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)读取传感器记录％| readdetectionsfile |功能读取录制的传感器数据文件。记录的数据是一个单独的结构，它被分为%以下结构:％％＃| InertialMeasurementUnit |，带字段的结构数组：时间戳，%速度和yawRate。数组中的每个元素都对应于a%不同的步伐。％＃| LaneReports |，带字段的结构数组：左右。每个元素数组的%对应不同的时间步长。% left和right都是带字段的结构:isValid, confidence，%边界类型、偏移量、航向角和曲率。% # |radarObjects|，一个包含以下字段的结构数组:% numObjects (integer)和对象(struct)。数组中的每个元素％对应于不同的时间表。|是一个结构数组，其中每个元素是一个单独的对象，% with fields: id, status, position(x;y;z)， velocity(vx,vy,vz)，%振幅和rangeMode。%注意:z总是常数，vz=0。％＃| VisionObjects |，带字段的STRUCT数组：时间戳（见下文），% 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车道和保持一个持续的自我车道估计

函数[巷道，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。方向角== -1e9 ||右曲率= = 1 e9);如果Cond egolane.right =施法（[rightlane.curvature，rightlane.headingangangle，rightlane.offset]，“双”)；结束％更新右侧车道边界参数或使用前一个rightparams = egolane.right;RightBoundaries = ParaboliclaneBoundary（RightParams）;RightBoundaries.strength = 1;Laneboundaries = [LeftBoundaries，RightBoundaries];结束

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

函数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 = Union（......相交（查找（NORMVS> MINV），查找（inzone == 1）），......找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

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

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

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

函数postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)%将视频对象处理为对象检测对象numradarobjects = numel（后处理Ddetections）;NumVisionObjects = VisionFrame.Numobjects;如果NumVisionObjects Classtouse = class（VisionFrame.Object（1）.Position）;VisionMeascov =铸造（Diag（[2,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)%将雷达对象处理为对象检测对象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）;Object.velocity（2）]，......“SensorIndex”2,“MeasurementNoise”radarMeasCov,......“MeasurementParameters”{2},......“ObjectAttributes”,{对象。id、对象。状态对象。振幅,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在这个前向碰撞预警例子中使用的测量函数的雅可比矩阵

函数fcwmeasjac(state, sensorID)%示例测量依赖于传感器类型，由% objectDetection的MeasurementParameters属性。我们选择对于雷达对象的视频对象和Sensorid = 2的％Sensorid = 1。的%以下两个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;结束结束结束