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

打开脚本

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

合成数据生成的设置场景

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

%用于重复结果rng (2019);创建场景，自我车辆，获取雷达和激光雷达传感器[scenario, egoVehicle, radar, lidar] = helperCreateRadarLidarScenario;

Ego车辆安装有四个2-D雷达传感器。前后雷达传感器的视野为45度。左右雷达传感器的视野为150度。每台雷达的方位角分辨率为6度，射程为2.5米。Ego还安装了一个3d激光雷达传感器，其方位为360度，仰角为40度。激光雷达的方位角分辨率为0.2度，仰角分辨率为1.25度(32个仰角通道)。在下面的动画中可视化传感器的配置和模拟传感器数据。注意，雷达具有比物体更高的分辨率，因此返回每个物体的多个测量值。还要注意的是，激光雷达与演员的低多边形网格以及路面相互作用，从这些物体中返回多个点。

雷达跟踪算法

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

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

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

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

radarTrackingAlgorithm = helperRadarTrackingAlgorithm(雷达);

激光雷达跟踪算法

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

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

使用激光雷达数据跟踪物体的算法被封装在helper类中，helperLidarTrackingAlgorithm作为系统对象实现。这个类输出一个数组objectTrack对象，并根据以下约定定义其状态:

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

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

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidar);

设置Fuser, Metrics和可视化

熔化炉

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

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

radarConfig = fusersourcecconfiguration (“SourceIndex”, 1…“IsInitializingCentralTracks”,真的,…“CentralToLocalTransformFcn”@central2radar,…“LocalToCentralTransformFcn”, @radar2central);

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

lidarConfig = fusersourcecconfiguration ()“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} $$ 是每个估计的混合系数。通常，这些混合系数是通过最小化融合协方差的行列式或迹来估计的。在本例中，通过最小化每个估计的位置协方差的行列式来估计混合权重。此外，由于雷达不估计三维状态，三维状态只与激光雷达融合。有关详细信息，请参阅helperRadarLidarFusionFcn函数，如脚本末尾所示。

接下来，使用trackFuser对象。

%中央航迹状态空间与激光雷达航迹状态空间相同;%因此使用相同的状态转换函数。函数是%在helperLidarTrackingAlgorithm类中定义。f = lidartrackingalgorithm . stattransitionfcn;创建一个trackFuser对象fuser = trackFuser“SourceConfigurations”{radarConfig; lidarConfig},…“StateTransitionFcn”f…“ProcessNoise”，diag([1 31 1])，…“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);%激光雷达GOSPAtrackgospalidar =“距离”，“自定义”，…“DistanceFcn”@helperLidarDistance,…“CutoffDistance”25);%中央/融合GOSPAgospaCentral = trackGOSPAMetric(“距离”，“自定义”，…“DistanceFcn”@helperLidarDistance,…%状态空间与激光雷达相同“CutoffDistance”25);

可视化

此示例的可视化是使用helper类实现的helperLidarRadarTrackFusionDisplay．显示器分为4个面板。显示器绘制了来自每个传感器的测量和轨道以及融合的轨道估计。显示的图例如下所示。此外，音轨以其独特的身份(TrackID)以及前缀。前缀“R”、“L”和“F”分别代表雷达、激光雷达和融合估计。

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

运行场景和跟踪器

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

所有跟踪算法的GOSPA度量及其组成部分。Gospa = 0 (3,0);missTarget = 0 (3,0);falseTracks = 0 (3,0);%初始化fusedTracksfusedTracks = objectTrack.empty(0,1);%用于存储gospa指标所经过的时间步长的计数器。Idx = 1;指标的基本真实值。这个变量每个时间步更新一次%自动成为actor的句柄。groundTruth = scenario.Actors(2:end);而推进(场景)%当前时间time = scenario.SimulationTime;%收集雷达和激光雷达测量数据和自我姿态跟踪%场景框架。参见下面的helperCollectSensorData。[radarDetections, ptCloud, egoPose] = helperCollectSensorData(egoVehicle，雷达，激光雷达，时间);%生成雷达轨迹radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, time);%生成激光雷达轨迹和分析信息，如边界框%检测和点云分割信息[lidarTracks, lidarDetections, segmentationInfo] =…lidarTrackingAlgorithm(egoPose, ptCloud, time);%连接雷达和激光雷达轨迹localTracks =[雷达轨迹;激光轨迹];%更新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);%更新显示显示(场景、雷达、雷达探测、雷达轨迹、…lidar, ptCloud, lidarDetections, segmentinfo, lidarTracks，…fusedTracks);%更新用于存储GOSPA指标的索引Idx = Idx + 1;结束%更新示例动画updateExampleAnimations(显示);

评估性能

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

跟踪维护

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

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

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

Track-level准确性

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

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

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

间隔太近的目标

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

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

由于两个主要原因，雷达轨迹在此事件期间不受影响。首先，雷达传感器在每次检测中输出距离-速率信息，这些信息与行驶较慢的汽车相比，在噪声级别之外是不同的。这将导致单个车辆检测之间的统计距离增加。其次，扩展目标跟踪器根据预测轨迹评估多个可能的聚类假设，从而拒绝不合适的聚类并接受合适的聚类。请注意，为了使扩展对象跟踪器正确选择最佳集群，跟踪过滤器必须具有一定的鲁棒性，以便能够捕获两个集群之间的差异。例如，具有高过程噪声和高度不确定维度的轨道可能无法正确声明群集，因为它的过早年龄和较高的灵活性来解释不确定事件。

远距离目标

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

总结

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

效用函数

collectSensorData

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

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

radar2cental

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

函数centralTrack = radar2central(radarTrack)初始化正确状态大小的轨道centralTrack = objectTrack(“状态”1) 0(10日,…“StateCovariance”、眼睛(10));radarTrack的%同步属性，除了State和StateCovariance% radarTrack参见下面定义的同步轨道。centralTrack = syncTrack(centralTrack，雷达轨道);xRadar =雷达轨道状态;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*xRadar;%线性状态转换PCentral = H*PRadar*H';线性协方差变换PCentral([6 7 10]，[6 7 10]) = eye(3);%未观察状态%设置中央轨道的状态和协方差centralTrack。State = xCentral;centralTrack。StateCovariance = PCentral;结束

central2radar

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

函数radarTrack =中央雷达(centralTrack)初始化正确状态大小的轨道radarTrack = objectTrack(“状态”1) 0(7日,…“StateCovariance”、眼睛(7));除了State和StateCovariance之外，中心化轨道的%同步属性% radarTrack参见下面定义的同步轨道。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;%线性状态转换= H*PCentral*H';线性协方差变换%设置雷达航迹的状态和协方差radarTrack。State = xRadar;radarTrack。StateCovariance = PRadar;结束

syncTrack

一个将一个轨道的属性与另一个轨道同步的函数，除了“State”和“StateCovariance”属性

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

构成

一个函数，以结构形式返回自我飞行器的姿态。

函数egoPose = pose(egoVehicle)Position = egoVehicle.Position;egoPose。Velocity = egoVehicle.Velocity;egoPose。Yaw = egoVehicle.Yaw;egoPose。Pitch = egoVehicle.Pitch;egoPose。Roll = 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 = [norm(truth.Velocity(1:2));truth.Velocity(3)];%实际偏航actYaw = truth.Yaw;%实际尺寸。actDim = [truth.Length;truth.Width;truth.Height];%实际偏航率actYawRate = truth.AngularVelocity(3);计算误差在每一个估计加权的“需求”%的系统。在每个方面使用马氏距离指定的距离其中协方差是由“需求”定义的。这%有助于避免轨道在其下方/上方报告时的倾斜距离由于状态/测量模型的不准确性导致的不确定性。%位置错误。estPos = track。State([1 2 6]);reqPosCov = 0.1*eye(3);e = estPos - actPos;d1 = sqrt(e'/reqPosCov*e);%速度误差estVel =轨道。状态(7 [3]);reqVelCov = 5*eye(2);e = estVel - actVel;d2 = sqrt(e'/reqVelCov*e);偏航误差%estYaw = track.State(4);reqYawCov = 5;e = estYaw - actYaw;d3 = sqrt(e'/reqYawCov*e);%偏航率错误estYawRate = track.State(5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 = sqrt(e'/reqYawRateCov*e);%尺寸错误estDim = track。State([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);%仅2-D%实际速度actVel = norm(truth.Velocity(1:2));%实际偏航actYaw = truth.Yaw;%实际尺寸。只有二维雷达actDim = [truth.Length;truth.Width];%实际偏航率actYawRate = truth.AngularVelocity(3);计算误差在每一个估计加权的“需求”%的系统。在每个方面使用马氏距离指定的距离其中协方差是由“需求”定义的。这%有助于避免轨道在其下方/上方报告时的倾斜距离由于状态/测量模型的不准确性导致的不确定性。%位置错误estPos = track。状态([1 - 2]);reqPosCov = 0.1*eye(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 - actYaw;d3 = sqrt(e'/reqYawCov*e);%偏航率错误estYawRate = track.State(5);reqYawRateCov = 1;e = estYawRate - actYawRate;d4 = sqrt(e'/reqYawRateCov*e);%尺寸错误estDim = track。状态(7 [6]);reqDimCov = eye(2);e = estDim - actDim;d5 = sqrt(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 = PAll(:，:，1);onlyLidarStates = false(10,1);onlyLidarStates([6 7 10]) = true;仅将此信息与激光雷达融合xOnlyLidar = xAll(onlyLidarStates，:);POnlyLidar = PAll(onlyLidarStates,onlyLidarStates，:);%与雷达和激光雷达相交的状态和协方差xToFuse = xAll(~onlyLidarStates，:);PToFuse = PAll(~onlyLidarStates，~onlyLidarStates，:);%行列式的排序顺序。这有助于按顺序构建具有可比测定值的%协方差。例如，两个大的%的协方差可能相交到一个较小的协方差，这与%协方差第三小。为i = 1:n dets(i) = det(PToFuse(1:2,1:2,i));结束[~，idx] = sort()“下”）;xToFuse = xToFuse(:，idx);topfuse = topfuse (:，:，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(:，valid);PMerge = POnlyLidar(:，:，valid);如果sum(valid) > 1 [xL,PL] = fusecovint(xMerge,PMerge);elseifsum(valid) == 1 xL = xMerge;PL = PMerge;其他的xL = 0 (3,1);PL = eye(3);结束x(onlyLidarStates) = xL;P(onlyLidarStates,onlyLidarStates) = 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”。然而。在这种情况下，w是通过最小化来选择的只有“位置”协方差的决定因素。N = size(x1,1);Idx = [1 2];detP1pos = det(P1(idx,idx));detP2pos = det(P2(idx,idx));w1 = detP2pos/(detP1pos + detP2pos);w2 = detP1pos/(detP1pos + detP2pos);I = eye(n);P1inv = I/P1;P2inv = I/P2;Pinv = w1*P1inv + w2*P2inv; P = I/Pinv; x = P*(w1*P1inv*x1 + w2*P2inv*x2);结束