雷达和激光雷达数据的轨迹级融合

这个示例使用:

打开脚本

此示例演示如何从雷达和激光雷达传感器的测量值生成对象级轨迹列表，并使用轨迹级融合方案对其进行进一步融合。使用扩展对象跟踪器处理雷达测量值，使用联合概率数据关联（JPDA）处理激光雷达测量值跟踪器。使用轨迹级融合方案进一步融合这些轨迹。工作流示意图如下所示。

合成数据生成的设置场景

使用此示例中使用的场景drivingScenario（自动驾驶工具箱）. 雷达和激光雷达传感器的数据是用计算机模拟的驱动雷达数据发生器（自动驾驶工具箱）和LidarPointCloudGenerator.（自动驾驶工具箱），分别。创建场景和传感器模型被包裹在辅助功能中helperCreateRadarLidarScenario．有关方案和合成数据生成的更多信息，请参阅以编程方式创建驾驶场景（自动驾驶工具箱）．

可重复结果的％rng (2020);创建场景，自我车辆，并获得雷达和激光雷达传感器[scenario, egoVehicle, radar, lidar] = helperCreateRadarLidarScenario;

ego车辆安装有四个二维雷达传感器。前后雷达传感器有45度的视野。左右雷达传感器的视野范围为150度。每个雷达的方位角分辨率为6度，射程为2.5米。ego还安装了一个三维激光雷达传感器，具有360度方位和40度仰角的视野。激光雷达的方位角分辨率为0.2度，仰角分辨率为1.25度(32个仰角通道)。在下面的动画中可视化传感器的配置和模拟传感器数据。注意，雷达的分辨率比物体高，因此对每个物体返回多个测量值。还请注意，激光雷达与actor的低聚网格以及道路表面相互作用，从这些对象返回多个点。

雷达跟踪算法

如上所述，雷达具有比物体更高的分辨率并返回每个对象的多个检测。诸如全局最近邻（GNN）和联合概率数据关联（JPDA）的传统跟踪器假设每个扫描每个对象的大多数检测器都返回。因此，在使用传统的跟踪器或必须使用扩展的对象跟踪器处理之前，必须从高分辨率传感器的检测群集。扩展对象跟踪器不需要预簇检测，并且通常估计运动状态（例如，位置和速度）以及物体的程度。有关传统跟踪器和扩展对象跟踪器之间的更详细比较，请参阅基于雷达和摄像头的公路车辆扩展目标跟踪的例子。

通常，扩展的对象跟踪器可以使用曲目的时间历史同时处理聚类和数据关联，更好地估计对象。在该示例中，使用高斯混合概率假设密度（GM-PHD）跟踪器（）处理雷达检测追踪器和gmphd)的矩形目标模型。有关配置跟踪器的更多细节，请参阅基于雷达和摄像头的公路车辆扩展目标跟踪的例子。

使用雷达测量跟踪目标的算法被包装在helper类中，帮助跟踪算法，作为系统对象实现™. 这个类输出一个数组对象跟踪对象并根据以下约定定义其状态：

$[x \ y \ s \ {\ theta} \ {omega} \ l \ w]$

RadarTrackingAlgorithm = HelperradarTrackingAlgorithm（雷达）;

激光雷达跟踪算法

与雷达类似，激光雷达传感器也会对每个物体返回多个测量值。此外，传感器从道路返回大量的点，这些点在用作目标跟踪算法的输入之前必须被删除。虽然扩展目标跟踪算法可以直接处理来自障碍物的激光雷达数据，但在激光雷达数据跟踪中，传统的跟踪算法仍较为普遍。出现这种趋势的第一个原因主要是针对大数据集的扩展目标跟踪器计算复杂度较高。第二个原因是对PointPillars[1]、VoxelNet[2]和PIXOR[3]等先进的基于深度学习的探测器的投资，这些探测器可以分割点云并返回车辆的包围盒检测。这些检测器有助于克服由于不适当的聚类而导致的传统跟踪器性能下降的问题。

在此示例中，使用配置有交互多模型（IMM）滤波器的传统联合概率数据关联（JPDA）跟踪器处理激光雷达数据。使用基于RANSAC的平面拟合算法对激光雷达数据进行预处理以去除点云，并通过执行基于欧几里得距离聚类算法形成边界框。有关该算法的更多信息，请参阅用激光雷达追踪车辆:从点云到轨迹表的例子。对比链接的示例，跟踪在场景框架中执行，跟踪器进行不同的调整以跟踪不同大小的对象。此外，对变量的状态进行了不同的定义，以约束轨道在其估计航向角方向上的运动。

使用激光雷达数据跟踪对象的算法被包装在helper类中，HelperlidartrackingAlgorithm.实现为System对象。此类输出数组对象跟踪对象并根据以下约定定义其状态：

$[x \ y \ s \ {\ theta} \ {\ omega} \ z \ {\ dot {z}} \ l \ w \ h]$

与雷达算法共同的状态类似地定义。此外，作为三维传感器，LIDAR跟踪器输出三个附加状态， $z$ 那 ${\dot{z}$ 和 $ h $ ，分别表示被跟踪对象的z坐标（m）、z速度（m/s）和高度（m）。

LidartTrackingAlgorithm=HelperLidartTrackingAlgorithm（激光雷达）；

设置Fuser、Metrics和Visualization

熔化炉

接下来，您将设置一个融合算法，用于融合来自雷达和激光雷达跟踪器的轨迹列表。与其他跟踪算法类似，建立轨迹级融合算法的第一步是为融合轨迹或中心轨迹定义状态向量（或状态空间）的选择。在这种情况下，融合轨迹的状态空间被选择为与激光雷达相同。选择中心轨迹状态空间后，将定义中心轨迹状态到本地轨迹状态的转换。在这种情况下，局部航迹状态空间指的是雷达和激光雷达航迹的状态。要做到这一点，您需要使用fusersourceconfiguration.对象

定义雷达源的配置。的帮助跟踪算法输出曲目sourceIndex.设置为1。将sourceIndex.作为每个跟踪器的属性提供，以唯一地标识它并允许融合算法区分来自不同源的曲目。因此，你设置了sourceIndex.雷达配置的特性与雷达轨迹的特性相同。你设置i初始化中央机架到符合事实的让未分配的雷达轨道启动新的中央轨道。接下来，您将中央状态空间中的轨道的转换定义为雷达状态空间，反之亦然。辅助功能central2radar和radar2central执行两个转换并包含在本示例的末尾。

radarConfig=熔丝资源配置(“源索引”，1，...“IsInitializingCentralTracks”符合事实的...“CentralToLocalTransformFcn”@central2radar,...'localtocentraltransformfcn'(中环),；

定义LIDAR源的配置。由于LIDAR轨道的状态空间与中央轨道相同，因此您没有定义任何转换。

lidarConfig = fuserSourceConfiguration (“源索引”2,...“IsInitializingCentralTracks”，对）；

下一步是定义状态融合算法。状态融合算法将中央状态空间中的多个状态和状态协方差作为输入，并返回状态和协方差的融合估计。在本例中，使用辅助函数提供的协方差求交算法，Helperradarlidarfusionfcn.．一个具有均值的高斯估计的一般协方差交算法 $x_i$ 和协方差 P_i美元可根据以下公式定义:

$美元P_ {F} ^ {1} = w_ {1} {P_ {1}} ^ {1} + w_ {2} {P_ {2}} ^ {1} $$

$x{F}=P{F}（w{1}{P{1}}}{-1}x{1}+w{2}{P{2}}{-1}x{2}）$

哪里美元间{F} $ 和 $P{F}$ 是融合状态和协方差和 $w_ {1}$ 和 $w_ {2}$ 是每个估计值的混合系数。通常，这些混合系数是通过最小化融合协方差的行列式或轨迹来估计的。在本例中，混合权重是通过最小化每个估计值的位置协方差的行列式来估计的。此外，由于雷达不估计三维状态，因此三维states仅与激光雷达融合。有关更多详细信息，请参阅Helperradarlidarfusionfcn.此脚本末尾显示的函数。

接下来，使用跟踪定影器对象

%中心轨道的状态空间与激光雷达的轨道相同，因此，％使用相同的状态转换函数。这个功能是%在helperLidarTrackingAlgorithm类中定义。f=LidartTrackingAlgorithm.StateTransitionFcn；%创建trackFuser对象熔化炉= trackFuser (“源配置”{radarConfig; lidarConfig},...“StateTransitionFcn”F...“ProcessNoise”诊断接头([1 3]),...“HasAdditiveProcessNoise”错误的...“AssignmentThreshold”，[250 inf]，...'确认察觉'[3 - 5],...“DeletionThreshold”,[5 5],...“StateFusion”那“习俗”那...'customstatefusionfcn', @helperRadarLidarFusionFcn);

指标

在本例中，您使用广义最优子模式分配度量(GOSPA)度量来评估每个算法的性能。您使用trackGOSPAMetric对于每个跟踪器。GOSPA指标旨在通过提供标量成本来评估跟踪系统的性能。指标值越低，表示跟踪算法的性能越好。

要将GOSPA度量用于自定义运动模型（如本例中使用的模型），请设置距离属性为“自定义”，并定义轨道与其相关地面真值之间的距离函数。这些距离函数，在这个例子的最后显示helperRadarDistance和助手身份．

%雷达GOSPAgosparadar = trackgospametric（“距离”那“习俗”那...“距离FCN”，@helperradardance，...'cutoffdistance',25);%激光雷达GOSPAgospaLidar=轨道GOSPAMETRIC(“距离”那“习俗”那...“距离FCN”@helperLidarDistance,...'cutoffdistance',25);中央/融合GOSPA %gospaCentral = trackGOSPAMetric (“距离”那“习俗”那...“距离FCN”@helperLidarDistance,...%状态空间与激光雷达相同'cutoffdistance',25);

可视化

使用辅助类实现此示例的可视化HelperLidarRackFusionDisplay. 显示屏分为4个面板。显示屏绘制每个传感器的测量值和轨迹以及融合轨迹估计值。显示屏的图例如下所示。此外，轨迹通过其独特的标识进行注释(探险网)以及前缀。前缀“R”、“L”和“F”分别代表雷达、激光雷达和融合估计。

％创建显示。%FollowActorID控制特写中显示的演员%展示display = helperlidarradartrackfusionDisplay（“FollowActorID”3);%显示永久性图例showLegend（显示、场景）；

运行场景和跟踪程序

接下来，您将推进该场景，从所有传感器生成合成数据，并对其进行处理，以从每个系统生成轨迹。您还可以使用场景中提供的地面真实值为每个跟踪器计算度量。

% Initialzie GOSPA度量及其组件的所有跟踪算法。gospa = 0 (0);missTarget = 0 (0);falseTracks = 0 (0);%初始化fusedTracksfusedTracks = objectTrack.empty (0,1);对于存储GOSPA指标的时间步骤的％A计数器。idx = 1;%度量标准的基本事实。这个变量在每个时间步更新%自动成为演员的把柄。背景真相=场景。参与者（2：结束）；虽然预付款（情景）%当前时间时间=scenario.SimulationTime；%收集雷达和激光雷达测量值和自我姿态，以便跟踪%的场景。请参阅下面的helperCollectSensorData。[radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle，雷达，激光雷达，时间);％生成雷达轨道雷达跟踪=雷达跟踪算法（电子姿态、雷达检测、时间）；%生成激光雷达轨迹和分析信息，如边界框%检测和点云分割信息[lidarTracks、Lidardetection、segmentationInfo]=...lidarTrackingAlgorithm (egoPose、ptCloud、时间);连接雷达和激光雷达跟踪localTracks=[radarTracks；lidarTracks]；%更新融合器。第一次调用必须包含一个本地曲目如果〜（isempty（localtracks）&&〜锁定（定影））fusedtracks = fuser（localtracks，time）;结束%捕获所有跟踪器的GOSPA及其组件[GOSPA（1，IDX），〜，〜，〜，misstarget（1，Idx），伪造物（1，Idx）] = Gosparadar（Radartracks，Tounttruth）;[GOSPA（2，IDX），〜，〜，〜，misstarget（2，Idx），伪造物（2，IDX）] = GOSPALTAR（Lidartracks，Tounttruth）;[GOSPA（3，IDX），〜，〜，〜，misstarget（3，IDX），伪造（3，IDX）] = GoSpentral（FusedTracks，Tounttruth）;%更新显示显示(场景，雷达，雷达探测，雷达跟踪，...激光雷达，ptCloud, lidardetection, segmentationInfo, lidarTracks，...fusedTracks);%更新用于存储GOSPA指标的索引Idx = Idx + 1;结束%更新示例动画更新示例动画（显示）；

评估绩效

使用可视化和定量指标评估每个跟踪器的性能。分析场景中的不同事件，了解轨道级融合方案如何帮助更好地估计车辆状态。

跟踪维护

下面的动画每三个时间步显示一次整个跑步过程。请注意，三个跟踪系统（雷达、激光雷达和航迹级融合）中的每一个都能够跟踪场景中的所有四辆车，并且没有确认任何虚假航迹。

您还可以使用GOSPA度量的“错过目标”和“错误跟踪”组件定量地度量性能的这一方面。注意在下图中，由于建立延迟，错过的目标组件从一个较高的值开始，并在每个跟踪系统大约5-10个步骤中下降到零。另外，请注意所有系统的假轨道分量为零，这表明没有确认假轨道。

%绘制错过的目标成分图;情节(missTarget ',“线宽”2);传奇('雷达'那激光雷达的那“融合”）;标题(“未命中目标指标”）;Xlabel（“时间步长”); 伊拉贝尔(“公制”）;网格在…上;%绘制假轨迹分量图;情节(falseTracks ',“线宽”2);传奇('雷达'那激光雷达的那“融合”）;标题(“假轨道度量”）;Xlabel（“时间步长”); 伊拉贝尔(“公制”）;网格在…上;

轨道水平精度

每个跟踪器的跟踪水平或定位精度也可以通过GOSPA度量在每个时间步进行定量评估。值越低表示跟踪精度越好。由于没有遗漏目标或假轨迹，该度量捕获了由每辆车的状态估计产生的定位误差。

注意，用于融合估计的GOSPA度量比用于单个传感器的度量要低，这表明从每个传感器的航迹估计融合后航迹精度提高了。

％plot gospa.图;情节(gospa ',“线宽”2);传奇('雷达'那激光雷达的那“融合”）;标题(“GOSPA公制”）;Xlabel（“时间步长”); 伊拉贝尔(“公制”）;网格在…上;

密集目标

如前所述，本示例使用基于欧氏距离的聚类和包围盒拟合，将激光雷达数据提供给传统跟踪算法。聚类算法通常会在对象距离很近的情况下出现问题。使用本例中使用的检测器配置，当通过的车辆接近ego车辆前面的车辆时，检测器将每个车辆的点云聚类成一个更大的边界框。你可以注意到，在下面的动画中，轨道偏离了车辆的中心。由于轨道在几个步骤的估计中具有较高的确定性，熔断估计最初也受到影响。然而，随着不确定性的增加，它与融合估计的关联变得越来越弱。这是因为协方差交集算法根据每个估计的确定性为每个分配的轨迹选择一个混合权值。

GOSPA度量中也捕捉到了这种效果。您可以在上面的GOSPA度量图中注意到，激光雷达度量显示了第65个时间步长附近的峰值。

由于两个主要原因，雷达轨迹在这次事件中没有受到影响。首先，雷达传感器在每次检测中都输出距离率信息，距离率信息不仅仅是过往车辆的噪声级，还与行驶较慢车辆的噪声级不同。这导致了从单个汽车检测到的统计距离的增加。其次，扩展目标跟踪器根据预测轨迹对多个可能的聚类假设进行评估，从而剔除不正确的聚类，接受正确的聚类。请注意，为了使扩展的目标跟踪器能够正确地选择最佳的集群，跟踪的过滤器必须健壮到能够捕获两个集群之间的差异的程度。例如，具有高过程噪声和高度不确定维度的轨道可能无法恰当地声明一个集群，因为它的年龄还不成熟，并且具有较高的灵活性来解释不确定事件。

远距离目标

由于目标从雷达传感器中取出，因此由于检测器处的信噪比和传感器的有限分辨率，测量的精度降低。这导致测量中的高不确定性，这反过来又降低了轨道精度。在特写镜头显示中注意到，雷达的轨道估计远离雷达传感器的地面真相，并以更高的不确定性报告。但是，LIDAR传感器在点云中报告足够的测量以生成“缩小”边界框。LIDAR跟踪算法测量模型中建模的收缩效果允许跟踪器保持具有正确尺寸的轨道。在这种情况下，激光雷达混合重量高于雷达，并且允许融合估计比雷达估计更准确。

总结

在本例中，您学习了如何设置用于融合来自雷达和激光雷达传感器的轨迹的轨迹级融合算法。您还学习了如何使用广义最优子模式度量及其相关组件评估跟踪算法。

效用函数

collectSensorData

用于生成当前时间步长的雷达和激光雷达测量值的函数。

作用[radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle，雷达，激光雷达，时间)%目标相对于车辆的当前姿态tgtPoses = targetPoses (egoVehicle);radarDetections =细胞(0,1);为i = 1:numel(雷达)thisRadarDetections = step(雷达{i}， tgtpose，时间);radarDetections = [radarDetections; thisRadarDetections];%#好的结束%从激光雷达生成点云RDMESH = ROADMESH（EGOVEHICLE）;ptcloud =步骤（延裙，Tgtposs，RDMesh，时间）;在情景框架中追踪自我车辆的％计算姿势。通常%通过INS系统获得。如果不可用，可以设置为%“原点”在ego车辆的框架中跟踪。egoPose =姿势(egoVehicle);结束

radar2cental

将雷达状态空间中的轨迹转换为中心状态空间中的轨迹的函数。

作用centralTrack = radar2central (radarTrack)初始化状态大小正确的轨道centralTrack = objectTrack (“状态”1) 0(10日,...“状态协方差”、眼睛(10));除了State和StateCovariance之外的radarTrack同步属性％RADARTRACTRACK请参阅下面定义的SYNCTRACK。centralTrack = syncTrack (centralTrack radarTrack);xRadar = radarTrack.State;PRadar = radarTrack.StateCovariance;H = 0(10、7);%雷达到中心线性变换矩阵H（1,1）=1；H（2,2）=1；H（3,3）=1；H（4,4）=1；H（5,5）=1；H（8,6）=1；H（9,7）=1；xCentral=H*X雷达；%线性状态变换PCentral = H * PRadar * H ';％线性协方差转换PCentral([6 7 10]，[6 7 10]) = eye(3);%未观测状态设置中心轨迹的状态和协方差centralTrack.State=xCentral；centralTrack.StateConvariance=PCentral；结束

central2radar

将中心状态空间中的轨迹转换为雷达状态空间中的轨迹的函数。

作用雷达支架=中央雷达（中央支架）初始化状态大小正确的轨道radarTrack = objectTrack (“状态”，零（7,1），...“状态协方差”、眼睛(7));%同步除State和StateCovariance的centralTrack属性％RADARTRACTRACK请参阅下面定义的SYNCTRACK。radarTrack = syncTrack (radarTrack centralTrack);xCentral = centralTrack.State;PCentral = centralTrack.StateCovariance;H = 0 (7, 10);中心到雷达的线性变换矩阵H（1,1）=1；H（2,2）=1；H（3,3）=1；H（4,4）=1；H（5,5）=1；H（6,8）=1；H（7,9）=1；xRadar=H*xCentral；%线性状态变换PRadar=H*PCentral*H'；％线性协方差转换%雷达航迹的设定状态和协方差radarTrack.State=xRadar；radarTrack.STATECONVALANCE=PRadar；结束

syncTrack

同步轨道属性的函数，除了“State”和“StateCovariance”属性

作用tr1 = syncTrack(tr1,tr2) props = properties(tr1);notState = ~ strcmpi(道具、“状态”）;notCov = ~ strcmpi(道具、“状态协方差”）;道具=道具（NOTSTATE＆NOTCOV）;为i = 1：numel（props）tr1。（道具{i}）= tr2。（道具{i}）;结束结束

姿势

作为结构的自我载体的返回姿态的函数。

作用我的意思是:位置= egoVehicle.Position;egoPose。速度= egoVehicle.Velocity;egoPose。偏航= egoVehicle.Yaw;egoPose。距= egoVehicle.Pitch;egoPose。滚= egoVehicle.Roll;结束

助手身份

函数，用于计算雷达状态空间中轨道的估计与指定的地面真值之间的归一化距离。

作用dist=helperLidarDistance（跟踪、真相）％计算跟踪器估计的状态的实际值% Center与origin不同，跟踪器估计中心rOriginToCenter=-truth.OriginOffset（：）+[0；0；truth.Height/2]；rot=四元数（[真值.偏航真值.俯仰真值.横滚]，'eulerd'那“ZYX股票”那“帧”); actPos=真值。位置（：）+旋转点（旋转，旋转中心“）；实际速度和z率actVel=[标准（真理.速度（1:2））；真理.速度（3）]；%实际偏航actYaw = truth.Yaw;%的实际尺寸。actDim=[真值.长度；真值.宽度；真值.高度]；实际偏航率actYawRate = truth.AngularVelocity (3);%计算每一个估计的误差加权的“要求”%系统。在各个方面使用马氏距离指定的距离估计值的百分比，协方差由“要求”定义。这个当轨道过低/过低报告时，有助于避免偏离距离%由于状态/测量模型的不精确性而产生的不确定性。%位置误差。estPos=轨道状态（[1 2 6]）；reqPosCov=0.1*眼（3）；e=estPos-actPos；d1=sqrt（e'/reqPosCov*e）；%速度误差Estvel = track.state（[3 7]）;Reqvelcov = 5 *眼睛（2）;e = estVel - actVel;d2 = sqrt（e'/ reqvelcov * e）;%偏航误差estyaw = track.state（4）;reqyawcov = 5;e = estyaw  -  actiaw;d3 = sqrt（e'/ reqyawcov * e）;％横摆率误差estYawRate = track.State (5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 =√e”/ reqYawRateCov * e);%尺寸误差estDim=轨道状态（[8 9 10]）；reqDimCov=眼睛（3）；e=estDim-actDim；d5=sqrt（e'/reqDimCov*e）；%总距离Dist = d1 + d2 + d3 + d4 + d5;结束

helperRadarDistance

函数，用于计算雷达状态空间中轨道的估计与指定的地面真值之间的归一化距离。

作用距离=帮助距离（轨迹、真相）％计算跟踪器估计的状态的实际值% Center与origin不同，跟踪器估计中心rOriginToCenter=-truth.OriginOffset（：）+[0；0；truth.Height/2]；rot=四元数（[真值.偏航真值.俯仰真值.横滚]，'eulerd'那“ZYX股票”那“帧”); actPos=真值。位置（：）+旋转点（旋转，旋转中心“）；actPos = actPos (1:2);%只有二维%实际速度actVel=标准（真理速度（1:2））；%实际偏航actYaw = truth.Yaw;%的实际尺寸。雷达只有二维actDim = [truth.Length; truth.Width];实际偏航率actYawRate = truth.AngularVelocity (3);%计算每一个估计的误差加权的“要求”%系统。在各个方面使用马氏距离指定的距离估计值的百分比，协方差由“要求”定义。这个当轨道过低/过低报告时，有助于避免偏离距离%由于状态/测量模型的不精确性而产生的不确定性。%位置误差estPos=轨道状态（[12]）；reqPosCov=0.1*眼（2）；e=estPos-actPos；d1=sqrt（e'/reqPosCov*e）；%的速度误差estVel = track.State (3);reqVelCov = 5;e = estVel - actVel;d2 = sqrt（e'/ reqvelcov * e）;%偏航误差estyaw = track.state（4）;reqyawcov = 5;e = estyaw  -  actiaw;d3 = sqrt（e'/ reqyawcov * e）;％横摆率误差estYawRate = track.State (5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 =√e”/ reqYawRateCov * e);%尺寸误差estDim =。状态(7 [6]);reqDimCov =眼(2);e = estDim - actDim;d5 =√e”/ reqDimCov * e);%总距离Dist = d1 + d2 + d3 + d4 + d5;如果没有测量三维状态，就会经常受到惩罚dist = dist + 3;结束

Helperradarlidarfusionfcn.

函数在中心轨道状态空间中融合状态和状态协方差

作用[x，P]=helperRadarLidarFusionFcn（xAll，PAll）n=size（xAll，2）；dets=0（n，1）；%初始化x和Px = xAll (: 1);P =笼罩(:,:1);onlyLidarStates = false (10, 1);onlyLidarStates([6 7 10]) = true;％仅限Lidar融合了这些信息xOnlyLidar = xAll (onlyLidarStates:);POnlyLidar =笼罩(onlyLidarStates、onlyLidarStates:);%与雷达和激光雷达相交的状态和协方差xToFuse = xAll (~ onlyLidarStates:);PToFuse =笼罩(~ onlyLidarStates ~ onlyLidarStates,:);%行列式排序的顺序。这有助于按顺序构建%具有可比测定值的协方差。例如，两个大的%协方差可能与一个更小的协方差相交，该协方差可与％第三最小协方差。为i=1:n dets（i）=det（PToFuse（1:2,1:2，i））；结束[〜，idx] = sort（dets，“下”）;xToFuse = xToFuse (:, idx);PToFuse = PToFuse (:,:, idx);初始融合估计thisx = xtofuse（：，1）;thisp = ptofuse（：：，1）;%序贯融合为i = 2:n [thisX,thisP] = fusecovinusingpos (thisX, thisP, xToFuse(:，i)， PToFuse(:，:，i));结束从所有来源分配熔融状态x（〜oyllidarstates）= thisx;p（〜onlylidarstates，〜onlylidarstates，:) = thispp;%仅使用激光雷达源融合某些状态有效=任何(abs(xOnlyLidar) > 1e-6,1);xMerge = xOnlyLidar(:,有效);PMerge = POnlyLidar(:,:有效);如果sum(valid) > 1 [xL,PL] = fusecovint(xMerge,PMerge);elseif总和（有效）==1 xL=xMerge；PL=PMerge；其他的xL = 0 (3,1);PL =眼(3);结束x（onlydarstates）=xL；P（onlydarstates，onlydarstates）=PL；结束作用[x，P]=fusecovintUsingPos（x1，P1，x2，P2）%协方差交集通常由以下公式使用%方程:% P^-1 = w1*P1^-1 + w2*P2^-1% x = P*(w1*P1^-1*x1 + w2*P2^-1*x2);%式中，w1+w2=1%通常是代表协方差矩阵的标量，如“det”或将P的% "trace"最小化以计算w。这是由函数提供的%“fusecovint”。然而在这种情况下，通过最小化%仅“位置”协方差的行列式。n =大小(x1, 1);Idx = [1 2];detP1pos =侦破(P1 (idx idx));detP2pos =侦破(P2 (idx idx));w1 = detP2pos/(detP1pos + detP2pos);w2 = (+ (+));我眼睛= (n);P1inv = I / P1;P2inv = I / P2;= w1*P1inv + w2*P2inv; P = I/Pinv; x = P*(w1*P1inv*x1 + w2*P2inv*x2);结束