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

这个示例使用:

打开脚本

本示例向您展示如何从雷达和激光雷达传感器的测量数据生成对象级跟踪列表，并使用跟踪级融合方案进一步融合它们。使用扩展的目标跟踪器处理雷达测量，使用联合概率数据关联(JPDA)跟踪器处理激光雷达测量。您可以使用声道级融合方案进一步融合这些声道。工作流的示意图如下所示。

看到基于ROS的雷达和激光雷达数据融合(ROS工具箱)本算法使用rosbag上的记录数据为例。

合成数据生成的设置场景

本例中使用的场景是使用drivingScenario(自动驾驶工具箱)．对雷达和激光雷达传感器的数据进行了模拟drivingRadarDataGenerator(自动驾驶工具箱)而且lidarPointCloudGenerator(自动驾驶工具箱),分别。场景和传感器模型的创建封装在helper函数中helperCreateRadarLidarScenario．有关场景和合成数据生成的更多信息，请参见以编程方式创建驾驶场景(自动驾驶工具箱)．

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

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

雷达跟踪算法

如前所述，雷达比目标有更高的分辨率，并返回多个探测每个目标。传统的跟踪器，如全球最近邻(GNN)和联合概率数据关联(JPDA)，假设传感器每次扫描最多返回一个探测对象。因此，来自高分辨率传感器的检测必须在传统跟踪器处理之前进行聚类，或者必须使用扩展目标跟踪器进行处理。扩展的目标跟踪器不需要预聚类检测，通常估计运动状态(例如，位置和速度)和目标的范围。有关传统跟踪器和扩展对象跟踪器之间的更详细的比较，请参阅基于雷达和摄像机的公路车辆扩展目标跟踪的例子。

一般来说，扩展的对象跟踪器提供了更好的对象估计，因为它们使用跟踪的时间历史同时处理聚类和数据关联。在本例中，雷达探测使用高斯混合概率假设密度(GM-PHD)跟踪器(trackerPHD而且gmphd)，目标模型为矩形。有关配置跟踪器的更多详细信息，请参阅基于雷达和摄像机的公路车辆扩展目标跟踪的例子。

使用雷达测量跟踪目标的算法被包装在助手类中，helperRadarTrackingAlgorithm，作为一个系统对象™实现。的数组objectTrack对象并根据以下约定定义它们的状态:

$[x\ y\ s\ {\theta}\ {\omega}\ L\ W]$

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(雷达);

激光雷达跟踪算法

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

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

使用激光雷达数据跟踪对象的算法被包装在助手类中，helperLidarTrackingAlgorithm实现为系统对象。的数组objectTrack对象，并根据以下约定定义其状态:

$美元[x \ \ s \{\θ}\{ω\}\ z \{\点{z}} \ H L \ W \]美元$

雷达算法的公共状态定义类似。此外，作为一个3d传感器，激光雷达跟踪器可以输出三种额外的状态， z美元， ${\点{z}}$ 而且 H美元，分别为被跟踪物体的z坐标(m)、z速度(m/s)和高度(m)。

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(激光雷达);

设置Fuser，指标和可视化

熔化炉

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

定义雷达源的配置。的helperRadarTrackingAlgorithm输出跟踪与SourceIndex设置为1。的SourceIndex作为每个跟踪器上的属性提供，以惟一地识别它，并允许融合算法从不同的源区分跟踪。因此，您设置SourceIndex雷达配置的属性与雷达轨迹的属性相同。你设置IsInitializingCentralTracks来真正的让未分配的雷达轨迹启动新的中心轨迹。接下来，定义中央状态空间中的轨迹到雷达状态空间的转换，反之亦然。辅助功能central2radar而且radar2central执行这两个转换，它们包含在本示例的末尾。

radarConfig = fuserSourceConfiguration (“SourceIndex”，1,.．.“IsInitializingCentralTracks”,真的,.．.“CentralToLocalTransformFcn”@central2radar,.．.“LocalToCentralTransformFcn”, @radar2central);

定义激光雷达源的配置。由于激光雷达轨迹的状态空间与中心轨迹相同，因此不需要定义任何转换。

lidarConfig = fuserSourceConfiguration (“SourceIndex”2,.．.“IsInitializingCentralTracks”,真正的);

下一步是定义状态融合算法。状态融合算法以中心状态空间中的多个状态和状态协方差为输入，返回状态和协方差的融合估计。在这个例子中，你使用辅助函数提供的协方差交集算法，helperRadarLidarFusionFcn．具有均值的两个高斯估计的一般协方差交集算法 x_i美元和协方差 P_i美元可根据以下公式定义:

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

$F F $间{}= P_ {} (w_ {1} {P_{1}} ^{1}间的{1}+ w_ {2} {P_{2}} ^{1}间的{2})美元$

在哪里美元间{F} $ 而且 $美元P_ {F} $$ 融合状态和协方差和 $美元w_ {1} $$ 而且 $美元w_ {2} $$ 是来自每个估计的混合系数。通常，这些混合系数是通过最小化行列式或融合协方差的迹来估计的。在这个例子中，混合权重通过最小化每个估计的位置协方差的行列式来估计。此外，由于雷达不估计3-D状态，3-D状态只与激光雷达融合。有关更多细节，请参阅helperRadarLidarFusionFcn此脚本末尾显示的函数。

接下来，使用集合所有信息trackFuser对象。

中心航迹的状态空间与激光雷达上的航迹相同，%因此您使用相同的状态转换函数。函数是%定义在helperLidarTrackingAlgorithm类中。f = lidarTrackingAlgorithm.StateTransitionFcn;创建一个trackFuser对象熔化炉= trackFuser (“SourceConfigurations”{radarConfig; lidarConfig},.．.“StateTransitionFcn”f.．.“ProcessNoise”诊断接头([1 3]),.．.“HasAdditiveProcessNoise”假的,.．.“AssignmentThreshold”, 250年正无穷,.．.“ConfirmationThreshold”[3 - 5],.．.“DeletionThreshold”, 5 [5],.．.“StateFusion”，“自定义”，.．.“CustomStateFusionFcn”, @helperRadarLidarFusionFcn);

指标

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

要将GOSPA度量与自定义运动模型(如本例中使用的)一起使用，您需要设置距离属性设置为'custom'，并定义轨迹与其相关的地面真值之间的距离函数。这些距离函数，在这个例子的最后显示是helperRadarDistance,helperLidarDistance．

%雷达GOSPAgospaRadar = trackGOSPAMetric (“距离”，“自定义”，.．.“DistanceFcn”@helperRadarDistance,.．.“CutoffDistance”25);%激光雷达GOSPAgospaLidar = trackGOSPAMetric (“距离”，“自定义”，.．.“DistanceFcn”@helperLidarDistance,.．.“CutoffDistance”25);中央/融合GOSPA %gospaCentral = trackGOSPAMetric (“距离”，“自定义”，.．.“DistanceFcn”@helperLidarDistance,.．.状态空间和激光雷达一样“CutoffDistance”25);

可视化

此示例的可视化是使用一个助手类实现的helperLidarRadarTrackFusionDisplay．显示屏分为4个面板。显示器绘制来自每个传感器的测量和轨迹以及融合轨迹估计。显示的图例如下所示。此外，这些音轨都有它们独特的标识(TrackID)以及前缀。前缀“R”、“L”和“F”分别代表雷达、激光雷达和融合估计。

%创建一个显示。% FollowActorID控制特写中显示的演员%显示显示= helperLidarRadarTrackFusionDisplay (“FollowActorID”3);%显示持久传说showLegend(显示、场景);

运行场景和跟踪器

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

初始化所有跟踪算法的GOSPA度量及其组件。gospa = 0 (0);missTarget = 0 (0);falseTracks = 0 (0);%初始化fusedTracksfusedTracks = objectTrack.empty (0,1);用于存储gospa指标所消耗的时间步骤的计数器。idx = 1;%度量的基本真理。该变量每次更新一次时间步%自动成为演员的句柄。groundTruth = scenario.Actors(2:结束);而推进(场景)%当前时间时间= scenario.SimulationTime;收集雷达和激光雷达的测量数据，摆姿势跟踪%的场景。请参阅下面的helperCollectSensorData。[radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle，雷达，激光雷达，时间);%生成雷达轨迹radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, time);生成激光雷达轨迹和分析信息，如包围框%检测和点云分割信息[lidarTracks, lidarDetections, segmentationInfo] =.．.lidarTrackingAlgorithm (egoPose、ptCloud、时间);连接雷达和激光雷达轨迹。localTracks = [radarTracks; lidarTracks];%更新fuser。第一次呼叫必须包含一个本地轨道如果~(isempty(localTracks) && ~isLocked(fuser)) fusedTracks = fuser(localTracks,time);结束为所有跟踪器捕获GOSPA及其组件[gospa(1,idx)，~，~，~，missTarget(1,idx)，falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth);[gospa(2,idx)，~，~，~，missTarget(2,idx)，falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth);[gospa(3,idx)，~，~，~，missTarget(3,idx)，falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth);%更新显示显示(场景，雷达，雷达探测，雷达轨迹，雷达.．.激光雷达，ptCloud, lidarDetections, segmentationInfo, lidarTracks，.．.fusedTracks);更新用于存储GOSPA指标的索引Idx = Idx + 1;结束%更新示例动画updateExampleAnimations(显示);

评估性能

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

跟踪维护

下面的动画每三个时间步显示整个运行过程。请注意，三种跟踪系统——雷达、激光雷达和跟踪级融合——都能够跟踪场景中的所有四辆车辆，并且没有确认错误的跟踪。

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

绘制未命中的目标组件图;情节(missTarget ',“线宽”2);传奇(“雷达”，激光雷达的，“融合”）;标题(“错过了目标指标”）;包含(“时间步”）;ylabel (“指标”）;网格在；绘制假跟踪组件图;情节(falseTracks ',“线宽”2);传奇(“雷达”，激光雷达的，“融合”）;标题(“错误跟踪指标”）;包含(“时间步”）;ylabel (“指标”）;网格在；

Track-level准确性

每个跟踪器的跟踪级或定位精度也可以通过每个时间步上的GOSPA度量进行定量评估。值越小，跟踪精度越高。由于没有错过目标或错误的跟踪，该度量捕获了由每辆车的状态估计造成的定位错误。

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

%绘制GOSPA图;情节(gospa ',“线宽”2);传奇(“雷达”，激光雷达的，“融合”）;标题(“GOSPA指标”）;包含(“时间步”）;ylabel (“指标”）;网格在；

间隔太近的目标

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

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

雷达轨迹在这次事件中没有受到影响，主要有两个原因。首先，雷达传感器在每次探测中输出距离-速率信息，这些信息在经过的汽车与慢速行驶的汽车相比的噪音水平之外是不同的。这就增加了从单个车辆检测到的统计距离。其次，扩展目标跟踪器根据预测轨迹评估多个可能的聚类假设，结果是拒绝不适当的聚类，接受适当的聚类。注意，对于要正确选择最佳集群的扩展对象跟踪器，跟踪的过滤器必须健壮到能够捕获两个集群之间的差异的程度。例如，一个具有高过程噪声和高度不确定维度的轨道可能无法恰当地声明一个集群，因为它的年龄过早，在考虑不确定事件时具有更高的灵活性。

远距离目标

由于目标远离雷达传感器，由于探测器的信噪比降低和传感器的有限分辨率，测量的准确性下降。这导致了测量的高度不确定性，进而降低了跟踪精度。请注意在下面的特写显示中，雷达的轨迹估计距离雷达传感器的地面真相更远，报告具有更高的不确定性。然而，激光雷达传感器在点云中报告了足够的测量结果，从而生成一个“缩小”的边界框。激光雷达跟踪算法的测量模型中引入了收缩效应，使得跟踪器保持了正确尺寸的轨迹。在这种情况下，激光雷达混合权重高于雷达，并允许融合估计比雷达估计更准确。

总结

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

效用函数

collectSensorData

在当前时间步上生成雷达和激光雷达测量的功能。

函数[radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle，雷达，激光雷达，时间)当前目标相对于自我载具的姿态tgtPoses = targetPoses (egoVehicle);radarDetections =细胞(0,1);为i = 1: numl (radars) thisRadarDetections = step(radars{i}， tgtphotos,time);radarDetections = [radarDetections; thisRadarDetections];% #好< AGROW >结束%从激光雷达生成点云rdMesh = roadMesh (egoVehicle);ptCloud =步骤(激光雷达，tgtpose, rdMesh，时间);在场景框架中计算自我车辆的跟踪姿态。通常%使用INS系统获取。如果不可用，可以将其设置为在自我车辆的框架中跟踪“原点”。egoPose =姿势(egoVehicle);结束

radar2cental

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

函数centralTrack = radar2central (radarTrack)初始化一个状态大小正确的轨道。centralTrack = objectTrack (“状态”1) 0(10日,.．.“StateCovariance”、眼睛(10));radarTrack除State和StateCovariance外的%同步属性参见下面定义的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;6 H(8日)= 1;7 H(9日)= 1;xCentral = H * xRadar;线性状态变换PCentral = H * PRadar * H ';%线性协方差变换PCentral([6 7 10]，[6 7 10]) =眼(3);%的国家设置中心轨迹的状态和协方差centralTrack。状态= xCentral; centralTrack.StateCovariance = PCentral;结束

central2radar

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

函数radarTrack = central2radar (centralTrack)初始化一个状态大小正确的轨道。radarTrack = objectTrack (“状态”1) 0(7日,.．.“StateCovariance”、眼睛(7));同步除State和StateCovariance之外的centralTrack的%属性参见下面定义的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。状态= xRadar; radarTrack.StateCovariance = PRadar;结束

syncTrack

同步一个音轨与另一个音轨的属性的函数状态而且StateCovariance属性。

函数tr1 = syncTrack(tr1,tr2) props = properties(tr1);notState = ~ strcmpi(道具、“状态”）;notCov = ~ strcmpi(道具、“StateCovariance”）;props = props(notState & notCov);为i = 1:元素个数(道具)tr1。(道具{我})= tr2。(道具{我});结束结束

构成

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

函数egoPose = pose(egoVehicle) egoPose。位置= egoVehicle.Position;egoPose。速度= egoVehicle.Velocity;egoPose。偏航= egoVehicle.Yaw;egoPose。距= egoVehicle.Pitch;egoPose。滚= egoVehicle.Roll;结束

helperLidarDistance

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

函数dist = helperLidarDistance(track, truth)计算跟踪器估计的状态的实际值。%中心不同于原点，跟踪器估计中心rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2];腐烂=四元数([真理。偏航真理。球场上的真理。卷),“eulerd”，“ZYX股票”，“帧”）;actPos = true . position (:) + rotatepoint(rot,rOriginToCenter')';%实际速度和z速率actVel =[规范(truth.Velocity (1:2)); truth.Velocity (3)];%实际偏航actYaw = truth.Yaw;%的实际尺寸。actDim = [truth.Length; truth.Width truth.Height];%实际偏航速率actYawRate = truth.AngularVelocity (3);%计算误差在每个估计加权的“需求”%的系统。用马氏距离在各个方面指定的距离%的估计，其中协方差由“需求”定义。这%有助于避免偏离距离时，航迹低于/超过报告他们%不确定度，因为状态/测量模型不准确。%位置错误。estPos =。状态([1 2 6]); reqPosCov = 0.1*eye(3); e = estPos - actPos; d1 = sqrt(e'/reqPosCov*e);%速度误差estVel =。状态(7 [3]);reqVelCov = 5 *眼(2);e = estVel - actVel;d2 =√e”/ reqVelCov * e);%偏航误差estYaw = track.State (4);reqYawCov = 5;e = estYaw - actYaw;d3 =√e”/ reqYawCov * e);%偏航率误差estYawRate = track.State (5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 =√e”/ reqYawRateCov * e);%尺寸错误estDim =。状态([8 9 10]); reqDimCov = eye(3); e = estDim - actDim; d5 = sqrt(e'/reqDimCov*e);%总距离Dist = d1 + d2 + d3 + d4 + d5;结束

helperRadarDistance

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

函数dist = helperRadarDistance(track, truth)计算跟踪器估计的状态的实际值。%中心不同于原点，跟踪器估计中心rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2];腐烂=四元数([真理。偏航真理。球场上的真理。卷),“eulerd”，“ZYX股票”，“帧”）;actPos = true . position (:) + rotatepoint(rot,rOriginToCenter')';actPos = actPos (1:2);%只有二维%的实际速度actVel =规范(truth.Velocity (1:2));%实际偏航actYaw = truth.Yaw;%的实际尺寸。雷达只有二维actDim = [truth.Length; truth.Width];%实际偏航速率actYawRate = truth.AngularVelocity (3);%计算误差在每个估计加权的“需求”%的系统。用马氏距离在各个方面指定的距离%的估计，其中协方差由“需求”定义。这%有助于避免偏离距离时，航迹低于/超过报告他们%不确定度，因为状态/测量模型不准确。%位置错误estPos =。状态([1 - 2]);reqPosCov = 0.1 *眼(2);e = estPos - actPos;d1 =√e”/ reqPosCov * e);%的速度误差estVel = track.State (3);reqVelCov = 5;e = estVel - actVel;d2 =√e”/ reqVelCov * e);%偏航误差estYaw = track.State (4);reqYawCov = 5;e = estYaw - actYaw;d3 =√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);依据= 0 (n, 1);初始化x和Px = xAll (: 1);P =笼罩(:,:1);onlyLidarStates = false (10, 1);onlyLidarStates([6 7 10]) = true;只使用激光雷达来融合这些信息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] =排序(侦破,“下”）;xToFuse = xToFuse (:, idx);PToFuse = PToFuse (:,:, idx);%初始化融合估算thisX = xToFuse (: 1);thisP = PToFuse (:: 1);%序贯融合为i = 2:n [thisX,thisP] = fusecovintUsingPos(thisX, thisP, xToFuse(:，i)， PToFuse(:，:，i));结束从所有源分配融合状态x (~ onlyLidarStates) = thisX;P (~ onlyLidarStates ~ onlyLidarStates,:) = thisP;只使用激光雷达源融合一些状态valid = any(abs(xOnlyLidar) > 1e-6,1);xMerge = xOnlyLidar(:,有效);PMerge = POnlyLidar(:,:有效);如果sum(valid) > 1 [xL,PL] = fusecovint(xMerge,PMerge);elseifsum(valid) == 1 xL = xMerge;PL = PMerge;其他的xL = 0 (3,1);PL =眼(3);结束x (onlyLidarStates) = xL;P (onlyLidarStates onlyLidarStates) = PL;结束函数(x, P) = fusecovintUsingPos (P1 x1, 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”。然而。在这种情况下，选择w是通过最小化“位置”协方差的决定因素。。n =大小(x1, 1);Idx = [1 2];detP1pos =侦破(P1 (idx idx));detP2pos =侦破(P2 (idx idx));w1 = detP2pos/(detP1pos + detP2pos);w2 = detP1pos/(detP1pos + detP2pos);我眼睛= (n);P1inv = I / P1;P2inv = I / P2;Pinv = w1*P1inv + w2*P2inv; P = I/Pinv; x = P*(w1*P1inv*x1 + w2*P2inv*x2);结束

参考文献

[1] Lang, Alex H.，等。PointPillars:用于点云对象检测的快速编码器。IEEE计算机视觉和模式识别会议论文集，2019。

周，尹，还有Oncel Tuzel。Voxelnet:基于点云的三维物体检测的端到端学习。IEEE计算机视觉与模式识别会议论文集，2018。

[3]杨，斌，罗文杰，和Raquel Urtasun。Pixor:从点云中实时三维物体检测。IEEE计算机视觉与模式识别会议论文集，2018。