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

这个示例使用:

打开脚本

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

合成数据生成的设置方案

本示例中使用的场景是使用drivingScenario．对雷达和激光雷达传感器的数据进行了仿真drivingRadarDataGenerator和lidarPointCloudGenerator,分别。场景和传感器模型的创建包含在helper函数中Helpercreateradarlidarscenario.．有关场景和合成数据生成的更多信息，请参见以编程方式创建驾驶场景．

%用于可重复性结果rng (2019);创建场景，自我车辆，并获得雷达和激光雷达传感器[情景，Egovehicle，雷达，LIDAR] = Helpercreateradarlidarscenario;

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

雷达跟踪算法

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

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

使用雷达测量跟踪对象的算法封装在helper类中，helperRadarTrackingAlgorithm，实现为System对象™。这个类输出一个数组objectTrack(传感器融合与跟踪工具箱)对象，并根据以下约定定义它们的状态:

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

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(雷达);

激光雷达跟踪算法

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

在本例中，激光雷达数据使用传统的联合概率数据关联(JPDA)跟踪器进行处理，该跟踪器配置了一个交互多模型(IMM)滤波器。采用基于ransac的平面拟合算法对激光雷达数据进行去除点云的预处理，采用基于欧氏距离聚类算法形成包围盒。有关该算法的更多信息，请参阅使用LIDAR跟踪车辆：从点云到跟踪列表(传感器融合与跟踪工具箱)例子。与链接示例进行比较，在场景帧中执行跟踪，并且跟踪器被不同地调整以跟踪不同大小的对象。此外，变量的状态不同地定义，以限制轨道的运动在其估计的标题角度的方向上。

使用LIDAR数据跟踪对象的算法包装在辅助课程中，helperLidarTrackingAlgorithm实现为系统对象。这个类输出一个数组objectTrack(传感器融合与跟踪工具箱)对象，并根据以下约定定义它们的状态:

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

相似地定义了雷达算法的常见状态。此外，作为一个三维传感器，激光雷达跟踪器输出另外三种状态， z美元， ${\点{z}}$ 和 H美元，分别为被跟踪物体的z坐标(m)、z速度(m/s)和高度(m)。

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(激光雷达);

设置定影，指标和可视化

熔化炉

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

定义雷达源的配置。的helperRadarTrackingAlgorithm输出跟踪与SourceIndex设置为1。的SourceIndex是作为每个跟踪器的属性提供的，以唯一地识别它，并允许融合算法区分来自不同来源的跟踪。因此，设置SourceIndex雷达配置的属性与雷达轨道一样。你设置了IsInitializingCentralTracks来真正的让那些未分配的雷达轨迹启动新的中央轨迹。接下来，定义中心状态空间中的轨迹到雷达状态空间的转换，反之亦然。辅助功能central2radar和雷达2中央执行两个转换，并在该示例的末尾包括。

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

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

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

下一步是定义状态融合算法。状态融合算法以中心状态空间中的多个状态和状态协方差为输入，返回状态和协方差的融合估计。在本例中，使用助手函数提供的协方差交集算法，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} $$ 是每个估计的混合系数。通常，这些混合系数是通过最小化行列式或融合协方差的轨迹来估计的。在这个例子中，混合权重是通过最小化每个估计的位置协方差的行列式来估计的。此外，由于雷达不估计三维状态，三维状态只与激光雷达融合。有关详情，请参阅helperRadarLidarFusionFcn函数显示在这个脚本的末尾。

接下来，您可以使用a组装所有信息trackFuser对象。

%中央轨迹的状态空间与激光雷达的轨迹相同，因此你使用相同的状态转移函数。函数是升降机内部级别中的％定义。f = lidarTrackingAlgorithm.StateTransitionFcn;%创建trackFuser对象熔化炉= trackFuser (“SourceConfigurations”，{radarconfig; lidarconfig}，.．.“StateTransitionFcn”f.．.“ProcessNoise”，诊断（[1 3 1]），.．.“HasAdditiveProcessNoise”假的,.．.“AssignmentThreshold”, 250年正无穷,.．.“ConfirmationThreshold”[3 - 5],.．.'deletionthreshold', 5 [5],.．.'立式'，“自定义”，.．.“CustomStateFusionFcn”, @helperRadarLidarFusionFcn);

指标

在此示例中，您可以使用广义最佳的子模式分配度量（GOPPA）度量来评估每种算法的性能。您使用三个单独的指标设置trackgometric.(传感器融合与跟踪工具箱)每个追踪器。GOSPA指标旨在通过提供标量成本来评估跟踪系统的性能。度量值越低，跟踪算法的性能越好。

要使用自定义运动模型(如本例中使用的模型)使用GOSPA度量，可以设置距离属性为“自定义”，并定义轨道与其相关地面真值之间的距离函数。这些距离函数，在这个例子的最后显示赫尔珀拉德斯坦,helperLidarDistance．

%雷达GOSPAgospaRadar = trackGOSPAMetric (“距离”，“自定义”，.．.“DistanceFcn”@helperRadarDistance,.．.“CutoffDistance”25);％LIDAR GoSPA.gospaLidar = trackGOSPAMetric (“距离”，“自定义”，.．.“DistanceFcn”，@ helperlidardistance，.．.“CutoffDistance”25);％中/融合GOSPAGoSpentral = TrackGoSpametric（“距离”，“自定义”，.．.“DistanceFcn”，@ helperlidardistance，.．.状态空间与激光雷达相同“CutoffDistance”25);

可视化

本示例的可视化是使用helper类实现的helperLidarRadarTrackFusionDisplay．显示屏分为4个面板。显示器绘制从每个传感器的测量和轨迹以及融合轨迹估计。显示的图例如下所示。此外，轨道是通过它们的独特身份进行注释的(TrackID）以及前缀。“R”，“L”和“F”代表分别用于雷达，激光雷达和融合估计。

%创建一个显示。% FollowActorID控制特写中显示的角色%显示显示= helperLidarRadarTrackFusionDisplay (“FollowActorID”3);显示持久化图例showLegend(显示、场景);

运行方案和跟踪器

接下来，您提前的场景，从所有传感器生成合成数据，并处理它以从每个系统生成曲目。您还使用场景中可用的地面真实来计算每个跟踪器的度量标准。

% Initialzie GOSPA度量及其组件的所有跟踪算法。GOSPA =零（3,0）;misstarget = zeros（3,0）;falsetracks = zeros（3,0）;％初始化fusedTracks.fusedtracks = ObjectTrack.empty（0,1）;%用于存储gospa指标的时间步长计数器。Idx = 1;指标的％地面真相。此变量每次更新一次%自动成为角色的句柄。groundTruth = scenario.Actors(2:结束);而推进(场景)%当前时间时间= scenario.SimulationTime;收集雷达和激光雷达测量值和自我姿态来跟踪％场景框架。请参阅下面的HelpercollectSendata。[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)，假轨道(2,idx)] = gospaLidar(lidarTracks, groundTruth);[gospa(3,idx)，~，~，~，missTarget(3,idx)，falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth);%更新显示显示（方案，雷达，radardetections，Radartracks，.．.激光雷达，ptCloud, lidardetection, segmentationInfo, lidarTracks，.．.熔化件）;%更新用于存储GOSPA指标的索引IDX = IDX + 1;结束%更新示例动画updateExampleAnimations(显示);

评估性能

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

跟踪维护

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

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

%绘制错过的目标成分图;情节(missTarget ',“线宽”2);传奇(“雷达”，'lidar'，“融合”）;标题（“错过了目标指标”）;包含(“时间步”）;ylabel (“指标”）;网格在；%绘制虚假轨道分量图;情节(falseTracks ',“线宽”2);传奇(“雷达”，'lidar'，“融合”）;标题（“错误跟踪指标”）;包含(“时间步”）;ylabel (“指标”）;网格在；

Track-level准确性

每个跟踪器的轨迹级别或定位精度也可以通过GOSPA度量在每个时间步骤进行定量评估。数值越小，跟踪精度越好。由于没有错过目标或错误轨迹，该度量捕获了由每辆车的状态估计导致的定位错误。

请注意，熔融估计的GOSPA度量低于单个传感器的度量，这表明在每个传感器的轨道估计融合后的轨道精度增加。

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

间隔太近的目标

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

GOSPA指标也捕捉到了这种效果。你可以注意到，在上面的GOSPA公制图中，激光雷达公制显示了大约在第65个时间步长的峰值。

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

远距离目标

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

概括

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

效用函数

collectSensorData

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

函数[radardetections，ptcloud，Efopose] = HelpercollectSensordata（Egovehicle，雷达，黎明，时间）目标相对于自我载体的当前姿态tgtposs =针织品（例如，Egovehicle）;radardetections =细胞（0,1）;为i = 1:numel(雷达)thisRadarDetections = step(雷达{i}， tgtpose，时间);radarDetections = [radarDetections; thisRadarDetections];% #好< AGROW >结束从LIDAR生成点云rdMesh = roadMesh (egoVehicle);ptCloud = step(lidar, tgtpose, rdMesh, time);%计算场景画面中自我车辆的姿态跟踪。通常％使用INS系统获得。如果不可用，这可以设置为在Ego的车架上追踪%“原产地”。egoPose =姿势(egoVehicle);结束

radar2cental

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

函数CentralTrack = Radar2Central（Radartrack）初始化状态大小正确的轨道CentralTrack = ObjectTrack（'状态'1) 0(10日,.．.“StateCovariance”、眼睛(10));除了State和StateCovariance之外的radarTrack同步属性% radarTrack见下面定义的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]) = eye(3);%的国家设置中心轨迹的状态和协方差centralTrack。状态= xCentral;centralTrack。StateCovariance = PCentral;结束

central2radar

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

函数radarTrack = central2radar (centralTrack)初始化状态大小正确的轨道RADARTRACK = ObjectTrack（'状态'1) 0(7日,.．.“StateCovariance”，眼睛（7））;%同步除State和StateCovariance的centralTrack属性% radarTrack见下面定义的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.

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

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

构成

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

函数EGOPOSE =姿势（例如，EGOVEHICLE）EGOPOSE.POSTEN = EGOVEHICLE.POST;EGOPOSE.VELOCITY = EGOVEHICLE.VELOCITY;Egopose.yaw = Egovehicle.waw;Egopose.pitch = Egovehicle.Pitch;Egopose.roll = Egovehicle.roll;结束

helperLidarDistance

函数来计算雷达状态空间和分配的地面真理中曲目的估计之间的归一化距离。

函数dist = helperLidarDistance(track, truth)%计算跟踪器估计的状态的实际值% Center与origin不同，跟踪器估计中心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 * (3);e = estPos - actPos;d1 =√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 =眼(3);e = estDim - actDim;d5 =√e”/ reqDimCov * e);%总距离Dist = d1 + d2 + d3 + d4 + d5;结束

赫尔珀拉德斯坦

函数来计算雷达状态空间和分配的地面真理中曲目的估计之间的归一化距离。

函数dist = helperRadarDistance(track, truth)%计算跟踪器估计的状态的实际值% Center与origin不同，跟踪器估计中心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 = track.state（[6 7]）;reqdimcov =眼睛（2）;e = estDim - actDim;d5 =√e”/ reqDimCov * e);%总距离Dist = d1 + d2 + d3 + d4 + d5;％不测量3-D状态的恒定性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] = fusecovinusingpos (thisX, thisP, xToFuse(:，i)， PToFuse(:，:，i));结束从所有来源分配熔融状态x (~ onlyLidarStates) = thisX;P (~ onlyLidarStates ~ onlyLidarStates,:) = thisP;只使用激光雷达源对某些州进行熔接有效=任何(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的％“跟踪”最小化以计算w。这是由该功能提供的%”fusecovint”。然而。在这种情况下，w是通过最小化只有“位置”协方差的行列式。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);结束