在ADAS/AD系统中，传感器融合是实现各传感器信息互补，提高目标状态精度，满足ISO-26262 ASIL功能安全等级的重要方法，其作用于感知层，作为后续规划与决策层，执行层的输入，重要性不言而喻。

　　传感器融合一般分为数据级融合，特征级融合，决策级融合。数据级融合一般用于同类型传感器融合；目前常见的 1R1V 或 5R1V 系统中采用 Vision 和 Radar 的融合， 以及 AD 系统中采用的 LiDAR 与 Radar 融合主要是特征级融合，即对多传感器目标状态信息进行融合；决策级融合一般针对具体功能利用不同传感器的决策与规划结果结合优先级在控制算法层面可以实现。

　　还记得之前一位做传感器融合的德国工程师给 OEM 介绍传感器融合直接使用了一词“Magic”。

　　MATLAB & Simulink提供了如GNN，JPDA，TOMHT，PHD，KF/EKF/UKF, PF, IMM等多种跟踪器及滤波器算法。

　　本文以Multiple Target Tracking点目标跟踪为例介绍下Vision和Radar融合“Magic”，后续再单独介绍扩展目标跟踪。

　　图1所示为多目标跟踪系统框架图，各传感器输出目标探测 Detections 经数据处理作为输入，Assignment 组件根据 Gating 组件中判断的门限范围内的 Detections 与 Tracks 进行目标关联；Track Maintenance组件对Tracks进行新增/更新/删除管理；Filtering组件通过测量值进行更新得到当前时刻 Tracks 的状态估计，并进行下一时刻的预测。

　　针对点目标跟踪MATLAB提供了trackerGNN，trackerJPDA，trackerTOMHT 实现上述框架算法，对应的Simulink模块如图2所示。可以直接在Simulink中如图3所示使用，输出即融合后的 Tracks，后续可以再根据具体功能选择CIPV或ACC targets。

　　下面具体来看下各个组件与其关键属性，以及在MATLAB中如何定义：

　　1. 传感器融合输入Detections

　　即对应各传感器的目标探测信息，MATLAB通过System Object将Vision及Radar的探测Detections打包成为objectDetection，方便调试各种跟踪器及滤波器算法，其包括如图4所示属性：

　　a). Measurement，不论是通过实车测试，还是通过虚拟场景及传感器模型都可以得到Vision与Radar基于时间序列的探测对象数据，主要包括x/y位置信息，vx/vy速度信息。Radar也可能发出探测目标在极坐标系下的径向距离rng，水平及垂直角度az，el，径向速度rr。Vision 输出的Detection都是针对单个目标的，Radar 如果针对一个目标可输出多个 Detections，则需要先进行聚类，如图 3 中 Detection Clustering 模块所示。

　　b). MeasurementNoise，非常重要的参数，和传感器测量特性及状态矩阵相关的对角矩阵，直接影响到最后融合状态估计量更相信 Vision 还是 Radar，以及是更相信测量值还是预测值。例如 Rradar = diag([1,1,2,10])；Rvision = diag([2,2,1,10]);

　　c). MeasurementParameters，可用于存取各传感器坐标系与车辆坐标系转换的信息，最终输出的 Detections 以车辆坐标系为基准。

　　2. 多目标跟踪门Gating及数据关联Assignment

　　在多目标跟踪中，为了减少不必要的数据关联计算，通过 Tracks 上一时刻状态预测当前时刻状态，建立当前时刻 Tracks 的有效跟踪门。利用门限来确定哪些 Detections 是 Tracks 的有效候选对象，在准确性可接受的范围内缩小门限值可加速数据关联。

　　由于传感器存在误报，漏报以及测量误差，同时也缺乏多目标环境的先验知识，可能多个 Detections 在同一个 Track 的门限内或者一个 Detection 在多个 Tracks 的门限内。如图 6 所示 O1~O4 是 Detections，T1~T3 是 Tracks，因此需要数据关联将 Vision 及 Radar 的探测 Detections 分别与 Tracks 进行关联或新建 Track。

　　图2中Simulink模块对应的数据关联算法即全局最近邻算法，联合概率数据关联，多假设跟踪数据关联。例如全局最近邻（GNN）数据关联算法：将门限内的一个Detection最多与一个Track关联，以使所有关联结果的总代价距离J 最小。

　　Cij 表示Detection i 与Track j之间的马氏距离，不同类别的探测目标（例如车辆和行人）之间，可以将其代价距离设置为较大值而不进行关联。对于 2-D GNN 分配问题，可以使用assignmunkres，assignauction，assignjv 等算法进行求解。

　　3. 目标跟踪管理 Track Maintenance

　　根据数据关联的结果，对有 detections 关联的 tracks 进行更新；将没有关联的探测 Detections 开始创建新的 Tracks，且对假设性的 Tracks 进行更新确认；对没有 Detections 关联的 Tracks 进行没有更新的预测或删除。

　　根据使用的关联算法可以选择不同的目标跟踪管理方法：trackerGNN 和 trackerJPDA 可以使用基于历史数据的方法（History Logic）确认，例如某个 Track 在 N 次更新中，至少有M次更新分配了 Detection，则可以确认。trackerGNN 和 trackerTOMHT 可以基于打分的方法（Track Score Logic）进行确认。

　　4. 滤波器 Filtering

　　虽然 Vision 和 Radar 可以对目标运动状态量进行测量，但由于噪声的存在，无法得到真值。实际项目中可以通过滤波对目标状态进行估计，尽量的逼近于真值。卡尔曼滤波根据状态方程和观测方程以递推的方式利用上一时刻的状态预测当前时刻的状态，并通过当前时刻的测量值对预测值进行更新得到当前时刻状态的最小方差估计。

　　图7为卡尔曼滤波算法公式，是针对零均值高斯噪声的纯时域滤波器。

　　那在 MATLAB 中如何根据下面公式创建一个卡尔曼滤波器？

　　步骤 1：需定义目标运动模型及对应的状态转移矩阵F，如常见匀加速模型或机动模型。

　　步骤 2：定义过程噪声 Q， sigma 为加速度变化率的最大值：

　　步骤 3：定义测量模型矩阵H，结合 Detections 从各传感器获得的测量值和状态转移矩阵而定：

　　步骤 4: 根据 Detections 输入状态和测量噪声确定目标初始状态及初始状态的协方差矩阵，每个时刻状态的不确定性都是通过状态协方差P矩阵（stateCov）来表示：

　　步骤 5：结合步骤1~4 创建滤波器初始化函数，用于新建 track：

　　还可尝试更多滤波器：Alpha-Beta 滤波/trackingABF, 卡尔曼/扩展卡尔曼/无迹卡尔曼/容积卡尔曼滤波/ trackingKF/trackingEKF/trackingUKF/trackingCKF，高斯和滤波/trackingGSF，粒子滤波/trackingPF，交互多模式滤波/trackingIMM 等。

　　完成上述各组件后就可创建用于多目标跟踪器 trackerGNN / trackerJPDA / trackerTOMHT，例如：

　　最后，可以使用 MATLAB 创建的多目标跟踪器或 Simulink 模块，可以自动生成 C/C++ 代码进行部署实现算法实时验证。