城市空中交通场景中的激光雷达和雷达融合

这个例子使用了:

打开实时脚本

本示例展示了如何在城市环境中使用多目标跟踪器跟踪各种无人机。属性创建场景uavScenario对象基于建筑物和地形数据可在线。然后使用激光雷达和雷达传感器模型生成合成传感器数据。最后，您使用各种跟踪算法来估计场景中所有无人机的状态。

无人机被设计用于广泛的作战。许多应用都是在城市环境中设置的，例如无人机包裹递送、空中出租车和电力线路检查。随着应用数量的增长，这些操作的安全性变得至关重要，这使得控制城市空域成为一项挑战。

创建城市空中交通场景

在本例中，您使用了Boulder, CO.的地形和建筑数据。数字地形高程数据(DTED)文件是从美国地质调查局提供的SRTM Void Filled数据集下载的。南博尔德的建筑数据.osm已从https://www.openstreetmap.org/该网站提供了获取世界各地众包地图数据的途径。这些数据是根据开放数据共享开放数据库许可证(ODbL)授权的，https://opendatacommons.org/licenses/odbl/．

dtedfile =“n39_w106_3arc_v2.dt1”；buildingfile =“southboulder.osm”；scene = createScenario(dtedfile,buildingfile);

接下来，在场景中添加一些无人机。

为了模拟一个包裹递送操作，定义一个从建筑物的屋顶出发并飞到另一个建筑物的轨迹。弹道由三条腿组成。四旋翼飞行器垂直起飞，然后飞向下一个交付目的地，最后垂直降落在屋顶上。

waypointsA = [1895 90 20;1915 108 35;1900 115 20];timeA = [0 25 50];trajA = waypointa“TimeOfArrival”赈灾发言人timeA,“ReferenceFrame”，“ENU表示”，“AutoBank”,真正的);uavA = uavPlatform(“无人机”场景,“轨迹”trajA,“ReferenceFrame”，“ENU表示”）;updateMesh (uavA“quadrotor”，{5}，[0.6350 0.0780 0.1840]，眼睛(4));

添加另一架无人机来模拟空中出租车飞过。它的轨迹是线性的，略有下降。使用fixedwing几何模型，以建立一个更大的无人机，适合运送人员。

waypointsB = [1940 120 50;1800 50 20];timeB = [0 41];trajB = waypointsB“TimeOfArrival”timeB,“ReferenceFrame”，“ENU表示”，“AutoBank”,真正的);uavB = uavPlatform(“UAV2”场景,“轨迹”trajB,“ReferenceFrame”，“ENU表示”）;updateMesh (uavB“fixedwing”，{10}，[0.6350 0.0780 0.1840]，眼睛(4));

然后添加一个四旋翼与轨迹遵循街道路径。这代表了一架无人机为了维护目的检查电网线路。

waypointsC = [1950 60 35;1900 60 35;1890 80 35];timeC = linspace(0,41,size(waypointsC,1));轨迹(waypointsC，“TimeOfArrival”timeC,“ReferenceFrame”，“ENU表示”，“AutoBank”,真正的);uavC = uavPlatform(“UAV3”场景,“轨迹”trajC,“ReferenceFrame”，“ENU表示”）;updateMesh (uavC“quadrotor”，{5}，[0.6350 0.0780 0.1840]，眼睛(4));

最后，添加自我无人机，无人机负责监视现场和跟踪不同的移动平台。

waypointsD = [1900 140 65;1910 100 65];计时= [0 60];轨迹(waypointsD，“TimeOfArrival”时间,.．.“ReferenceFrame”，“ENU表示”，“AutoBank”,真的,“AutoPitch”,真正的);egoUAV = uavPlatform(“EgoVehicle”场景,“轨迹”trajD,“ReferenceFrame”，“ENU表示”）;updateMesh (egoUAV“quadrotor”，{5}，[0 0 1]，眼睛(4));

定义无人机传感器套件

在自我飞行器上安装传感器。使用激光雷达冰球，通常用于汽车应用[1]。冰球是一个小型传感器，可以安装在四旋翼飞行器上。使用以下规格的激光雷达冰球:

距离分辨率:3厘米
最大射程:100米
360度方位角跨度，0.2°分辨率
仰角跨度30度，分辨率2°
更新频率:10hz
以90°倾斜安装向下看

在四旋翼上安装激光雷达。lidarOrient = [90 90 0];lidarSensor = uavLidarPointCloudGenerator(“MaxRange”, 100,.．.“RangeAccuracy”, 0.03,.．.“ElevationLimits”， [-15 15]，.．.“ElevationResolution”2,.．.“AzimuthLimits”， [-180 180]，.．.“AzimuthResolution”, 0.2,.．.“UpdateRate”10.．.“HasOrganizedOutput”、假);激光雷达“激光雷达”， egoUAV，激光传感器，“MountingLocation”， [0 0 -3]，“MountingAngles”, lidarOrient);

下一步，添加一个雷达使用radarDataGenerator雷达工具箱中的系统对象。要将该传感器添加到UAV平台，您需要定义一个自定义适配器类。有关详细信息，请参见模拟雷达传感器安装在无人机上(无人机工具箱)的例子。在本例中，使用helperRadarAdaptor类。这个类使用目标的网格几何来定义雷达模型的长方体尺寸。该网格还用于为每个目标推导一个简单的RCS签名。基于Echodyne EchoFlight无人机雷达[2]，设置雷达配置为:

频率:24.45-24.65 GHz
视野:120°方位角，80°仰角
分辨率:方位角2度，仰角6°
全扫描速率:1hz
灵敏度:0 dBsm在200米

此外，配置雷达输出多个探测每个对象。虽然雷达可以输出表示点目标的轨迹，但您希望估计目标的范围，这在默认轨迹输出中是不可用的。因此，请设置TargetReportFormat财产检测这样雷达就能直接报告粗略的探测结果。

在四旋翼上安装雷达。radarSensor = radardatgenerator (“没有扫描”，“SensorIndex”，1,.．.“FieldOfView”(120 80),.．.“UpdateRate”, 1.．.“MountingAngles”，[0 30 0]，.．.“HasElevation”,真的,.．.“ElevationResolution”6.．.“AzimuthResolution”2,.．.“RangeResolution”4.．.“RangeLimits”， [0 200]，.．.“ReferenceRange”, 200,.．.“CenterFrequency”24.55 e9,.．.“带宽”200 e6,.．.“TargetReportFormat”，“检测”，.．.“DetectionCoordinates”，“传感器矩形”，.．.“HasFalseAlarms”假的,.．.“FalseAlarmRate”1 e);雷达=无人机传感器“雷达”、egoUAV helperRadarAdaptor (radarSensor));

定义跟踪系统

激光雷达点云处理

激光雷达传感器返回点云。为了融合激光雷达输出，必须对点云进行聚类以提取目标检测。分割出地形使用segmentGroundSMRF函数从激光雷达工具箱。剩余的点云是聚类的，并且对每个聚类的平均高程应用一个简单的阈值来过滤建筑检测。用长方体对每个聚类进行拟合，提取边界盒检测。helper类helperLidarDetector本例中提供了实现细节。

激光雷达长方体探测使用objectDetection对象。这些检测的测量状态为 $［ x ， y ， z ， l ， W ， H ，问_{0} ，问_{1} ，问_{2} ，问_{3.} ］$ ,地点:

$x ， y ， z$ 分别为场景东、北、上(ENU)轴上的长方体中心坐标。
$l ， W ， H$ 分别为长方体的长、宽、高。
${问＝问}_{0} + 问_{1} ．我 + 问_{2} ． j + 问_{3.} ． k$ 是四元数，定义长方体相对于ENU轴的方向。

lidarDetector = helperLidarDetector(场景)

lidarDetector = helperLidarDetector with properties: MaxWindowRadius: 3 GridResolution: 1.5000 SegmentationMinDistance: 5 MinDetectionsPerCluster: 2 MinZDistanceCluster: 20 EgoVehicleRadius: 10

激光雷达跟踪

使用点目标跟踪器，trackerJPDA，用于跟踪激光雷达包围盒探测。点跟踪器假设每架无人机每次传感器扫描最多可以生成一个检测。这个假设是有效的，因为你已经把点云聚成了长方体。要设置跟踪器，您需要定义运动模型和测量模型。在本例中，您将使用增强恒速模型对无人机的动力学进行建模。匀速模型足以跟踪由直线飞行腿或缓慢变化段组成的轨迹。此外，假设无人机的方向不变，无人机的尺寸不变。得到了轨道状态和状态转移方程 $X ＝［ x ， v_{x} ， y ， v_{y} ， z ， v_{z} ， l ， W ， H ，问_{0} ，问_{1} ，问_{2} ，问_{3.} ］$ 而且

$X_{k + 1} ＝［ \begin{array}{c} ［ \begin{array}{c} 1 & t_{年代} & 0 & ． & ． & 0 \\ 0 & 1 & 0 & ． & ． \\ ． & ． & 1 & t_{年代} & ． & ． \\ ． & ． & 1 & 0 & 0 \\ ． & ． & 1 & t_{年代} \\ 0 & ． & ． & ． & 0 & 1 \end{array} ］ & 0_{3.} & 0_{3.} \\ 0_{3.} & 我_{3.} & 0_{3. x4} \\ 0_{3.} & 0_{4 x3} & 我_{4} \end{array} ］ X_{k} + 问_{k}$

在这里, $v_{x}$ ， $v_{y}$ ， $v_{z}$ 为沿场景ENU轴的长方体速度矢量坐标。跟踪方向使用四元数，因为欧拉角的不连续时，使用跟踪滤波器。 $t_{年代}$ ，更新的时间间隔k而且k + 1,等于0．1秒。最后, $问_{k}$ 是捕捉建模不准确性的附加过程噪声。

内部转换矩阵对应于匀速模型。的增强状态版本constvel而且cvmeas来解释额外的常数状态。具体实现在配套功能中initLidarFilter，augmentedConstvel，augmentedConstvelJac，augmentedCVmeas,augmentedCVMeasJac在这个例子的最后。

lidarJPDA = trackerJPDA(“TrackerIndex”2,.．.“AssignmentThreshold”(70 150),.．.“ClutterDensity”1 e-16.．.“DetectionProbability”, 0.99,.．.“DeletionThreshold”, 10 [10],.．.如果漏掉1秒，删除激光雷达轨迹“ConfirmationThreshold”[4 - 5],.．.“FilterInitializationFcn”@initLidarFilter)

lidarJPDA = trackerJPDA with properties: TrackerIndex: 2 FilterInitializationFcn: @initLidarFilter MaxNumEvents: Inf EventGenerationFcn: 'jpdaEvents' MaxNumTracks: 100 MaxNumDetections: Inf MaxNumSensors: 20 timetolance: 1.0000 -05 AssignmentThreshold: [70 150] InitializationThreshold: 0 DetectionProbability: 0.9900 ClutterDensity: 1.0000 -16 OOSMHandling: 'Terminate' TrackLogic: 'History' ConfirmationThreshold: [4 5] DeletionThreshold: [10 10] HitMissThreshold: 0.2000 HasCostMatrixInput:false HasDetectableTrackIDsInput: false statparameters: [1×1 struct] NumTracks: 0 NumConfirmedTracks: 0 EnableMemoryManagement: false

雷达跟踪

在本例中，假设雷达返回值经过预处理，只保留来自移动物体的返回值，也就是说，没有来自地面或建筑物的返回值。雷达测量状态为 $［ x ， v_{x} ， y ， v_{y} ， z ， v_{z} ］$ ．雷达分辨率很好，足以产生多个回报每个无人机目标，其检测不应该直接馈送到一个点目标跟踪器。有两种可能的方法来跟踪高分辨率雷达探测。其中一种方法是，您可以将检测聚类，并使用尺寸和方向常数增强状态，就像之前使用激光雷达长方体所做的那样。在另一种方法中，您可以使用GGIW-PHD跟踪器将检测提供给本例中采用的扩展目标跟踪器。该跟踪器使用逆Wishart分布估计每个目标的范围，其期望是一个3 × 3正定矩阵，表示目标的范围为3-D椭圆。第二种方法更可取，因为每个对象没有太多的检测，而且聚类的准确性不如扩展目标跟踪

要创建一个GGIW-PHD跟踪器，首先为每个报告给跟踪器的传感器定义跟踪传感器配置。在这种情况下，您只需要为一个雷达定义配置。当雷达安装平台移动时，您需要在每个跟踪器步骤之前将此配置更新为当前雷达姿态。接下来，根据传感器配置定义过滤器初始化函数。最后，构造一个trackerPHD对象，并增加分区阈值以捕获本示例中跟踪的对象的维度。实现细节显示在示例末尾的支持函数中createRadarTracker．

radarPHD = createRadarTracker(雷达传感器，egoUAV)

radarPHD = trackerPHD with properties: TrackerIndex: 1 SensorConfigurations: {[1×1 trackingSensorConfiguration]} PartitioningFcn: @(dets)partitionDetections(dets,threshold(1)，threshold(2)，'Distance'，'euclidean') MaxNumSensors: 20 MaxNumTracks: 1000 MaxNumComponents: 1000 AssignmentThreshold: 50 BirthRate: 1.0000e-03 DeathRate: 1.0000e-06 ExtractionThreshold: 0.8000 ConfirmationThreshold: 0.9900 DeletionThreshold: 0.1000 MergingThreshold: 50 LabelingThresholds: [1.0100 0.0100 0] StateParameters:[1×1 struct] HasSensorConfigurationsInput: true NumTracks: 0 NumConfirmedTracks: 0

跟踪融合

创建跟踪系统的最后一步是定义一个跟踪fuser对象来融合激光雷达跟踪和雷达跟踪。使用激光雷达航迹的13维状态作为融合状态定义。

radarConfig = fusersourcecconfiguration (“SourceIndex”，1,.．.“IsInitializingCentralTracks”,真正的);lidarConfig = fusersourcecconfiguration (“SourceIndex”2,.．.“IsInitializingCentralTracks”,真正的);fuser = trackFuser(“SourceConfigurations”{radarConfig, lidarConfig},.．.“ProcessNoise”blkdiag(2 *眼(6),1 *(3),0.2 *眼(4)),.．.“HasAdditiveProcessNoise”,真的,.．.“AssignmentThreshold”, 200,.．.“ConfirmationThreshold”[4 - 5],.．.“DeletionThreshold”, 5 [5],.．.“StateFusion”，“十字”，.．.“StateTransitionFcn”@augmentedConstvel,.．.“StateTransitionJacobianFcn”, @augmentedConstvelJac);

可视化

使用一个助手类来可视化该场景。本例中的helper类使用uavScenario可视化功能和theaterPlot绘图仪表示检测和跟踪信息。

显示屏分为5块，分别显示整体3d场景、3架无人机的3个跟踪摄像头和图例。

查看器= helperUAVDisplay(场景);

{“字符串”:“图无人机载激光雷达和雷达融合包含4轴对象和其他类型的uipanel对象。坐标轴对象1包含33个patch, line类型的对象。坐标轴对象2包含33个patch, line类型的对象。坐标轴对象3包含33个patch、line类型的对象。坐标轴对象4包含33个patch, line.

用于显示的雷达和激光雷达覆盖范围[radarcov,lidarcov] = sensorCoverage(雷达传感器，激光雷达);

模拟的场景

运行该场景并可视化跟踪系统的结果。每个目标的真实姿态以及雷达、激光雷达和融合航迹都被保存下来用于离线度量分析。

设置(现场);S = rng;rng (2021);numSteps = scene.StopTime*scene.UpdateRate;truthlog = cell(1,numSteps);radarlog = cell(1,numSteps);lidarlog = cell(1,numSteps);fusedlog = cell(1,numSteps);logCount = 0;而advance(scene) time = scene. currenttime;更新传感器读数并读取数据。updateSensors(现场);egoPose = read(egoUAV);%雷达跟踪[radardets, radarTracks, inforadar] = updateRadarTracker(雷达，radarPHD, egoPose，时间);用激光雷达跟踪[lidardets, lidarTracks, nonGroundCloud, groundCloud] = updateLidarTracker(激光雷达，lidarDetector, lidarJPDA, egoPose);引信激光雷达和雷达轨迹rectRadarTracks = formatPHDTracks(radarTracks);如果isLocked(fuser) || ~isempty(radarTracks) || ~isempty(lidarTracks) [fusedTracks，~，allfused,info] = fuser([lidarTracks;rectRadarTracks]，time);其他的fusedTracks = objectTrack.empty;结束%保存日志logCount = logCount + 1;lidarlog{logCount} = lidarTracks;radarlog{logCount} = rectRadarTracks;fusedlog{logCount} = fusedTracks;truthlog{logCount} = logTargetTruth(scene.Platforms(1:3));%更新数字viewer(radarcov, lidarcov, nonGroundCloud, groundCloud, lidardets, radardets, lidarTracks, radarTracks, fusedTracks);结束

{“字符串”:“图无人机载激光雷达和雷达融合包含4轴对象和其他类型的uipanel对象。Axes对象1包含37个patch、line、text类型的对象。坐标轴对象2包含37个patch、line、text类型的对象。Axes对象3包含37个patch、line、text类型的对象。坐标轴对象4包含37个类型为patch, line, text的对象

基于可视化结果，对跟踪性能进行初步的定性评估。场景最后的显示显示，这三架无人机都被自我很好地跟踪了。在当前的传感器套件配置下，由于激光雷达传感器的覆盖范围有限，激光雷达轨道只建立了部分。更宽的雷达视野允许在这种情况下建立更一致的雷达轨迹。

上面的三个动图显示了部分追逐视图。你可以看到激光雷达轨迹的质量(橙色框)受到场景几何形状的影响。无人机A(左)被激光雷达(黄色部分)几乎直接从上方照亮。这使得跟踪器能够随着时间的推移捕捉到无人机的全部范围。然而，无人机C(右)部分被雷达看到，这导致低估了无人机的尺寸。此外，估计的质心周期性地振荡周围的真实无人机中心。较大的固定翼无人机(中间)产生许多激光雷达点。因此，一旦目标完全进入激光雷达的视场，跟踪器可以检测和跟踪目标的全部范围。在这三种情况下，雷达(蓝色部分)提供了更准确的目标范围信息。因此，融合轨道盒(紫色)更紧密地捕捉每架无人机的范围。 However, the radar returns are less accurate in position. Radar tracks show more position bias and poorer orientation estimate.

跟踪指标

在本节中，您将使用OSPA(2)跟踪度量分析跟踪系统的性能。首先定义距离函数，该函数使用标量值量化轨道和真实值之间的误差。OSPA值越低，整体性能越好。

ospaR = trackOSPAMetric(“指标”，“OSPA(2)”，“距离”，“自定义”，“DistanceFcn”, @metricDistance);ospaL =克隆(ospaR);ospaF =克隆(ospaR);ospaRadar = 0 (1,numSteps);ospaLidar = 0 (1,numSteps);ospaFused = 0 (1,numSteps);为i=1:numSteps true = truthlog{i};ospaRadar(i) = ospaR(radarlog{i}，truth);ospaLidar(i) = ospaL(lidarlog{i}，truth);ospaFused(i) = ospaF(fusedlog{i}，truth);结束图绘制(ospaRadar,“颜色”,查看器。RadarColor,“线宽”2);持有在网格在情节(ospaLidar“颜色”,查看器。l我darColor,“线宽”2);情节(ospaFused“颜色”,查看器。FusedColor,“线宽”2);传奇(“雷达”，激光雷达的，“激光雷达+雷达”）;包含(“步骤”) ylabel (“OSPA(2)”）

图中包含一个轴对象。axis对象包含3个line类型的对象。这些对象代表雷达，激光雷达，激光雷达+雷达。

分析系统整体性能。即使目标无人机在传感器覆盖范围之外，每个跟踪器也会因没有跟踪任何无人机而受到惩罚。由于增加了监视区域，这表明在融合激光雷达和雷达时性能有所提高。这在模拟结束时尤其明显，两个目标被跟踪，一个由雷达跟踪，另一个由激光雷达跟踪，但两个目标都由热聚焦器跟踪。此外，可以看到融合后的OSPA低于激光雷达和雷达OSPA的最小值，表明融合后的航迹质量优于每个单独的航迹。

%清理removeCustomTerrain (“southboulder”）;rng(年代);

总结

本示例向您展示了如何建模无人机机载激光雷达和雷达跟踪系统，并在城市空中机动场景下对其进行测试。你使用了uavScenario目标是用地形和建筑创造一个逼真的城市环境。然后生成合成传感器数据来测试完整的跟踪系统链，包括点云处理、点目标和扩展目标跟踪以及跟踪融合。

支持功能

createScenario创建uavScenario使用OpenStreetMap地形和构建网格数据。

函数scene =创建场景(dtedfile,buildingfile)试一试addCustomTerrain (“southboulder”, dtedfile);抓%自定义地形已经添加。结束minHeight = 1.6925e+03;latlonCenter = [39.9786 -105.2882 minHeight];scene = uavScenario(“UpdateRate”10“StopTime”现年40岁的.．.“ReferenceLocation”, latlonCenter);%添加地形网格sceneXLim = [1800 2000];sceneYLim = [0 200];scene.addMesh (“地形”,{“southboulder”， sceneXLim, sceneYLim}，[0 0 0]);%添加建筑scene.addMesh (“建筑”，{建筑文件，sceneXLim, sceneYLim，“汽车”}， [0 0 0]);结束

createRadarTracker创建trackerPHD追踪器，以融合雷达探测。

函数跟踪器= createRadarTracker(雷达，egoUAV)为trackerPHD创建传感器配置fov = radar.FieldOfView;sensorLimits = [-fov(1)/2 fov(1)/2;视场(2)/ 2视场(2)/ 2;0正);sensor分辨率= [radar.AzimuthResolution;radar.RangeResolution];Kc = radar.FalseAlarmRate/(radar.AzimuthResolution*radar. rangerresolution *radar.ElevationResolution);Pd =雷达.探测概率;sensorPos = radar.MountingLocation(:);sensorOrient = rotmat(四元数雷达。MountingAngles,“eulerd”，“ZYX股票”，“帧”)，“帧”）;指定关于无人机的雷达帧信息sensorTransformParameters(1) = struct(“帧”，“球”，.．.“OriginPosition”sensorPos,.．.“OriginVelocity”0 (1),.．.传感器不会相对于自我移动“定位”sensorOrient,.．.“IsParentToChild”,真的,.．.帧旋转作为方向提供“HasElevation”,真的,.．.“HasVelocity”、假);指定UAV相对于场景的帧信息egoPose = read(egoUAV);sensorTransformParameters(2) = struct(“帧”，“矩形”，.．.“OriginPosition”egoPose (1:3).．.“OriginVelocity”egoPose(6节),.．.“定位”, rotmat(四元数(egoPose (13)),“帧”)，.．.“IsParentToChild”,真的,.．.“HasElevation”,真的,.．.“HasVelocity”、假);radarPHDconfig = trackingSensorConfiguration(雷达。SensorIndex,.．.“IsValidTime”,真的,.．.“SensorLimits”sensorLimits,.．.“SensorResolution”sensorResolution,.．.“DetectionProbability”帕金森病,.．.“ClutterDensity”Kc,.．.“SensorTransformFcn”@cvmeas,.．.“SensorTransformParameters”, sensorTransformParameters);radarPHDconfig。FilterInitializationFcn = @initRadarFilter;radarPHDconfig。MinDetectionProbability = 0.4;分区阈值阈值= [3 16];追踪器=追踪器“TrackerIndex”，1,.．.“HasSensorConfigurationsInput”,真的,.．.“SensorConfigurations”{radarPHDconfig},.．.“出生率”1 e - 3,.．.“AssignmentThreshold”, 50岁,.．.检测细胞添加出生成分的最小负对数可能性“ExtractionThreshold”, 0.80,.．.%要声明为轨迹的过滤器组件的权重阈值“ConfirmationThreshold”, 0.99,.．.%要声明为确认航迹的过滤器组件的权重阈值“MergingThreshold”, 50岁,.．.合并组件的阈值“DeletionThreshold”, 0.1,.．.%组件删除阈值“LabelingThresholds”，[1.01 0.01 0]，.．.这意味着没有轨道分裂。阅读LabelingThresholds帮助“PartitioningFcn”，@(dets) partitionDetections(dets, threshold(1)，threshold(2)，“距离”，“欧几里得”));结束

initRadarfilter实现了GGIW-PHD滤波器trackerPHD对象。在跟踪器更新期间使用此筛选器初始化密度中的新生成组件，并初始化检测分区中的新组件。

函数phd = initRadarFilter (detectionPartition)如果Nargin == 0%过程噪声sigP = 0.2;sigV = 1;Q = diag([sigP, sigV, sigP, sigV].^2);PhD = ggiwphd(zero (6,0)，repmat(eye(6)，[1 10 0])，.．.“ScaleMatrices”0 (3 3 0),.．.“MaxNumComponents”, 1000,.．.“ProcessNoise”问,.．.“HasAdditiveProcessNoise”,真的,.．.“MeasurementFcn”@cvmeas,.．.“MeasurementJacobianFcn”@cvmeasjac,.．.“PositionIndex”， [1 3 5]，.．.“ExtentRotationFcn”@ (x, dT)眼(3类(x)),.．.“HasAdditiveMeasurementNoise”,真的,.．.“StateTransitionFcn”@constvel,.．.“StateTransitionJacobianFcn”, @constveljac);其他的%nargin == 1% ------------------% 1)配置高斯混合% 2)配置逆向Wishart混合% 3)配置Gamma混合物% -----------------%% 1)配置高斯混合meanDetection = detectionPartition{1};n = numel(detectionPartition);收集所有测量和测量噪声。allDets = [detectionPartition{:}];zAll = horzcat(alldts . measurement);RAll = cat(3, alldds . measurementnoise);指定平均噪声和测量值z = mean(zAll,2);R = mean(RAll,3);meanDetection。测量= z;meanDetection。测量噪声= R;位置和速度协方差的解析均值检测。[posMeas,velMeas,posCov] = matlabshared.tracking.internal.fusion.parseDetectionForInitFcn(meanDetection，“initRadarFilter”，“双”）;创造恒定的速度状态和协方差状态= 0 (6,1);协方差= 0 (6,6);state (1:2:end) = posMeas;states(2:2:end) = velMeas;协方差(1:2:end,1:2:end) = posCov;协方差(2:2:end,2:2:end) = 10*eye(3);过程噪声%sigP = 0.2;sigV = 1;Q = diag([sigP, sigV, sigP, sigV].^2);2)配置逆向Wishart混合参数extent被设置为测量值在位置空间中的分布。e = zAll - z;Z = e*e'/n + R;Dof = 150;测量雅可比矩阵p = detectionPartition{1}.MeasurementParameters;H = cvmeasjac(状态，p);Bk = H(:，1:2:结束);Bk2 =眼(3)/Bk;V = (dof-4)*Bk2*Z*Bk2';配置伽马混合参数，使标准偏差检测次数的%为n/4Alpha = 16;%的形状Beta = 16/n;%的速度PhD = ggiwphd(.．..．.高斯参数州,协方差,.．.“HasAdditiveMeasurementNoise”,真的,.．.“ProcessNoise”问,.．.“HasAdditiveProcessNoise”,真的,.．.“MeasurementFcn”@cvmeas,.．.“MeasurementJacobianFcn”@cvmeasjac,.．.“StateTransitionFcn”@constvel,.．.“StateTransitionJacobianFcn”@constveljac,.．.“PositionIndex”，[1 3 5]，.．.“ExtentRotationFcn”， @(x,dT)眼(3)，.．..．.逆Wishart参数“DegreesOfFreedom”景深,.．.“ScaleMatrices”V,.．.“TemporalDecay”, 150,.．..．.γ参数“形状”α,“利率”,β,.．.“GammaForgettingFactors”, 1.05);结束结束

formatPHDTracks将椭圆GGIW-PHD航迹格式化为矩形增强状态航迹，用于航迹融合。convertExtendedTrack返回增强矩形状态的状态和状态协方差。用逆Wishart随机矩阵特征值来推导矩形盒的尺寸。特征向量提供方向四元数。在本例中，您对雷达轨迹尺寸和方向使用任意协方差，这通常足以用于跟踪。

函数tracksout = formatPHDTracks(tracksin)使用状态定义将跟踪结构从ggiwphd转换为objectTrack% [x y z vx vy vz L W H q0 q1 q2 q3]N =数字(tracksin);tracksout = repmat(objectTrack,N,1);为1:N tracksout(i) = objectTrack(tracksin(i));[state, statecov] = convertExtendedTrack(tracksin(i));tracksout(我)。状态=状态;tracksout(我)。StateCovariance = statecov;结束结束函数[state, statecov] = convertExtendedTrack(track)使用区段信息增加状态extent = track.Extent;[V,D] = eig(extent);%选择L > W > H.使用1.5 sigma作为维度[dims, idx] = sort(1.5*sqrt(diag(D))，“下”）;V = V(:，idx);q =四元数(V，“rotmat”，“帧”）;Q = Q /norm(Q);[q1, q2, q3, q4] =零件(q);state = [track.State;dim (:);第一季度;q2;第三季度;第四季度);Statecov = blkdiag(轨道。StateCovariance, 4*eye(3), 4*eye(4));结束

updateRadarTracker更新雷达跟踪链。该函数首先读取当前雷达返回值。然后雷达返回传递给GGIW-PHD跟踪器更新其传感器配置与自我无人机的当前姿态。

函数[radardets, radarTracks, inforadar] = updateRadarTracker(雷达，radarPHD, egoPose, time) [~，~，radardets， ~，~] = read(雷达);%已更新且时间输出与此工作流不兼容informadar = [];如果Mod (time,1) ~= 0 radardets = {};结束如果mod(time,1) == 0 && (isLocked(radarPHD) || ~isempty(radardets))更新跟踪器的雷达传感器配置configs = radarph . sensorconfigurations;款{1}.SensorTransformParameters(2)。OriginPosition = egoPose(1:3);款{1}.SensorTransformParameters(2)。OriginVelocity = egoPose(4:6);款{1}.SensorTransformParameters(2)。定向= rotmat(四元数(egoPose(10:13))，“帧”）;[radarTracks，~，~，inforadar] = radarPHD(radardets，配置，时间);elseifisLocked(radarPHD) radarTracks = predictTracksToTime(radarPHD)“确认”、时间);radarTracks = arrayfun(@(x) setfield(x，“UpdateTime”、时间)、radarTracks);其他的radarTracks = objectTrack.empty;结束结束

updateLidarTracker更新激光雷达跟踪链。该函数首先从激光雷达传感器读取当前点云输出。然后对点云进行处理，提取目标检测。最后，这些检测被传递给点目标跟踪器。

函数[lidardets, lidarTracks,nonGroundCloud, groundCloud] = updateLidarTracker(lidar, lidarDetector,lidarJPDA, egoPose) [~， time, ptCloud] = read(lidar);激光雷达总是在更新[lidardets,nonGroundCloud, groundCloud] = lidarDetector(egoPose, ptCloud,time);如果isLocked(lidarJPDA) || ~isempty(lidardets) lidarTracks = lidarJPDA(lidardets，时间);其他的lidarTracks = objectTrack.empty;结束结束

initLidarFilter初始化激光雷达跟踪器的过滤器。初始航迹状态由探测位置测量得到。速度被设置为0，协方差很大，以允许未来的检测与轨道相关联。这里还定义了增广状态运动模型、测量函数和雅可比矩阵。

函数ekf = initLidarFilter(检测)%激光雷达测量:[x y z L W H q0 q1 q2 q3]meas =检测。initState = [meas(1);0;meas(2);0;量(4:6);量(7:10)];initStateCovariance = blkdiag(100*eye(6)， 100*eye(3)， eye(4)));%过程噪声标准差sigP = 1;sigV = 2;sigD = 0.5;尺寸是恒定的，但部分可观察到sigQ = 0.5;Q = diag([sigP, sigV, sigP, sigV, sigP, sigV, sigD, sigD, sigQ, sigQ, sigQ].^2);ekf =跟踪ekf (“状态”initState,.．.“StateCovariance”initStateCovariance,.．.“ProcessNoise”问,.．.“StateTransitionFcn”@augmentedConstvel,.．.“StateTransitionJacobianFcn”@augmentedConstvelJac,.．.“MeasurementFcn”@augmentedCVmeas,.．.“MeasurementJacobianFcn”, @augmentedCVmeasJac);结束函数stateOut = augmentedConstvel(state, dt)%恒定速度的增广状态statout = constvel(state(1:6，:)，dt);stateOut = vertcat(stateOut,state(7:end，:));在预测阶段归一化四元数Idx = 10:13;qparts = statout (idx，:);N =平方根(sum(qparts.^2));Qparts = Qparts ./n;: stateOut (idx qparts (1) < 0) = -qparts (:, qparts (: 1) < 0);结束函数雅可比矩阵= augmentedConstvelJac(state,varargin)雅可比矩阵= constveljac(state(1:6，:)，varargin{:});雅可比矩阵= blkdiag(雅可比矩阵，眼(7));结束函数测量= augmentedCVmeas(状态)测量= cvmeas(状态(1:6，:));测量=[测量;:状态(七);:状态(13)];结束函数雅可比矩阵= augmentedCVmeasJac(state,varargin)雅可比矩阵= cvmeasjac(state(1:6，:)，varargin{:});雅可比矩阵= blkdiag(雅可比矩阵，眼(7));结束

sensorCoverage构造用于可视化的传感器覆盖配置结构。

函数[radarcov,lidarcov] = sensorCoverage(雷达传感器，激光雷达)radarcov = coverageConfig(雷达传感器);缩小覆盖范围以限制视觉混乱radarcov。范围= 10;lidarSensor = lidar.SensorModel;利达尔科夫=雷达;lidarcov。我ndex = 2; lidarcov.FieldOfView = [diff(lidarSensor.AzimuthLimits); diff(lidarSensor.ElevationLimits)]; lidarcov.Range = 5; lidarcov.Orientation = quaternion(lidar.MountingAngles,“eulerd”，“ZYX股票”，“帧”）;结束

logTargetTruth在整个仿真过程中记录真实的姿态和尺寸，用于性能分析。

函数logEntry = logTargetTruth(目标)n = nummel(目标);targetposed = repmat(struct(“位置”[],“速度”[],“维度”[],“定位”[]), 1, n);uavDimensions = [5 5 0.3;9.8 8.8 2.8;5 5 0.3];为I =1:n pose = read(目标(I));targetPoses(我)。位置=姿势(1:3);targetPoses(我)。速度=姿势(4:6);targetPoses(我)。维度= uavDimensions(i，:);targetPoses(我)。方位=姿势(10:13);targetPoses(我)。PlatformID = i;结束logEntry =目标姿势;结束

metricDistance为GOSPA定义自定义距离。这个距离包含了轨道在位置、速度、尺寸和方向上的误差。

函数out = metricDistance(track,truth) positionIdx = [1 3 5];velIdx = [2 4 6];dimIdx = 7:9;qIdx = 10:13;trackpos = track.State(positionIdx);trackvel = track.State(velIdx);trackdim = track.State(dimIdx);trackq =四元数(track.State(qIdx)');truepos =真理。位置;truevel =真理。速度; truedim = truth.Dimension; trueq = quaternion(truth.Orientation); errpos = truepos(:) - trackpos(:); errvel = truevel(:) - trackvel(:); errdim = truedim(:) - trackdim(:);以所需精度的倒数表示的权重Posw = 1/0.2;% m ^ 1Velw = 1/2;% (m/s) ^-1Dimw = 1/4;% m ^ 1Orw = 1/20;%度^ 1distPos = sqrt(errpos'*errpos);distVel = sqrt(errvel'*errvel);Distdim = sqrt(errdim'*errdim);Distq = rad2deg(dist(trackq, trueq));out = (distPos * posw + distVel * velw + distdim * dimw + distq * orw)/(posw + velw + dimw + orw);结束

参考文献

Velodyne激光雷达冰球:https://velodynelidar.com/2022世界杯八强谁会赢？products/puck/
Echodyne无人机雷达:https://www.echodyne.com/defense/uav-radar/