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

打开脚本

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

概述

前向碰撞预警(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;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,．..车道边界，传感器，确认轨道，最重要的对象，位置选择器，．..velocitySelector, visionObjects (currentStep), radarObjects (currentStep));%捕获快照如果time >= snapTime && time < snapTime + timeStep snapnow;结束结束

创建多对象跟踪器

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

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

FilterInitializationFcn:可能的运动和测量模型。在这种情况下，物体会有恒定的加速度运动。虽然你可以为这个模型配置一个线性卡尔曼滤波器，initConstantAccelerationFilter配置扩展卡尔曼滤波器。参见“定义卡尔曼滤波器”部分。
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],．..“DeletionThreshold”5);状态向量为:%匀速状态:State = [x;vx;y;vy]State = [x;vx;ax;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 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 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;%相对于测量噪声而言，数值很大stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2)， L, detection.MeasurementNoise(3:4,3:4)， L);步骤6:创建正确的过滤器。%使用'KF'表示trackingKF， 'EKF'表示trackingEKF，或'UKF'表示trackingUKFFilterType =“算法”；%创建过滤器:开关FilterType情况下“算法”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),．..“α”1 e 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];过滤器= trackingKF (“MotionModel”，“二维恒定加速度”，．..“MeasurementModel”H,“状态”、州、．..“MeasurementNoise”detection.MeasurementNoise (1:4, 1:4),．..“StateCovariance”, stateCov);结束结束

处理和格式化检测

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

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

函数[detections,laneBoundaries, 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_{结合}= 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;司机开始刹车前的延迟时间，以秒为单位position = getTrackPositions(confirmedTracks, positionSelector);velocity = gettrackvelocity (confirmedTracks, velocitySelector);为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 =“红色”；其他的%的谨慎结合= 2;threatColor =“黄色”；结束结束结束结束结束mostImportantObject =结构(“ObjectID”绪,“TrackIndex”trackID,“警告”结合,“ThreatColor”, threatColor);结束

总结

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

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

万博1manbetx支持功能

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

函数[visionObjects, radarObjects, inertialMeasurementUnit, laneReports，．..timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)读取传感器记录% |ReadDetectionsFile|函数用于读取记录的传感器数据文件。记录的数据是一个单独的结构，它被分为%以下结构:％% # |inertialMeasurementUnit|，一个带有字段的结构数组:timeStamp，%速度和yawRate。数组中的每个元素都对应于a%不同的步伐。% # |laneReports|，一个带有字段:left和right的结构数组。每个元素数组的%对应不同的时间步长。% 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|，一个带有字段的结构数组:% 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车道和保持一个持续的自我车道估计

函数[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);如果气孔导度egoLane。左= ([leftLane。曲率,leftLane。headingAngle, leftLane.offset],“双”）;结束%更新左车道边界参数或使用以前的参数leftParams = egoLane.left;leftBoundaries = parabolicLaneBoundary (leftParams);leftBoundaries。力量= 1;检查报告的右车道是否有效气孔导度= (rightLane。isValid && rightLane.confidence) &&．..~ (rightLane。方向角== -1e9 ||右曲率= = 1 e9);如果气孔导度egoLane。正确的= ([rightLane。曲率,rightLane。headingAngle, rightLane.offset],“双”）;结束%更新右车道边界参数或使用以前的参数rightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary (rightParams);rightBoundaries。力量= 1;[左边界，右边界];结束

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 =联盟(．..intersect(find(normVs > minV)， find(inZone == 1))，．..找到(inLane = = 1));realRadarObjects = radarObject (realRadarObjectsIdx);结束

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

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

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

函数postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)%将视频对象处理为对象检测对象numRadarObjects =元素个数(postProcessedDetections);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)．..[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 =元素个数(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;如果元素个数(状态)< 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，“喜欢”、州);如果元素个数(状态)< 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;结束结束结束