一、算法流程
沿用3D毫米波雷达的处理流程，4D雷达类似，如下图：
其中核心区别在于目标聚类开始的一系列处理，后面将重点描述。
二、4D毫米波雷达预处理
预处理主要包括标定、坐标转换和动静分离。
标定使用水平仪、角反，采集数据分析，得到水平和俯仰偏角。
坐标转换使用转换矩阵即可。
动静分离主要根据径向速度在车辆前进方向的投影进行判断。
三、4D毫米波雷达主处理
1.目标聚类
DBSCAN是经典的目标聚类算法，算法本身并不复杂，可参考这些文章
详解DBSCAN聚类_deephub的博客-CSDN博客_dbscan聚类。
对于4D雷达，更重要的是聚类之后的处理。
聚类后，可以得到基本信息，比如目标长度、宽度、高度、体积、投影面积、RCS以及各种统计分布。根据这些信息，可以对目标进行分类，得到目标的分类结果。
另外，可以估计目标的速度和朝向。
目标朝向角为α，目标点云在速度方向的投影即是点云的径向速度Vr，则有：
V(i)*cos(θ(i)-α) = Vr(i)
V(i) = Vr(i) / cos(θ(i)-α)
由于车辆是刚体，车身每个点速度相等，则有V(1) = V(2) = … = V(n)
V(i) = V(j) = V
Vr(i) / cos(θ(i)-α) = Vr(j) / cos(θ(j)-α)
可以计算得到点云的朝向角α，进而得到目标的真实速度和横纵向速度。
由于雷达对速度的测量可能存在模糊或出现错误，因此任意两个点的结果有较高错误概率，但多个点拟合得到的结果有较高的可行度，可以准确估计目标朝向和横纵向速度。
上文已经推导Vr(i) = V* cos(θ(i)-α) = V*(cos(θ(i))cos(α) + sin(θ(i))sin(α))
两边同时除以V*Vr(i)* cos(α)，则有
cos(θ(i))/Vr(i) = 1 / V cos(α) - tan(α) * sin(θ(i))* /Vr(i)
令y(i) = cos(θ(i))/Vr(i), x(i) = sin(θ(i))* /Vr(i)，b = 1 / V cos(α), k = - tan(α)则有
y(i) = k * x(i) + b
根据最小二乘拟合公式，可以得到V和α的估计值。
对于行人，由于不是刚体运动，并不适用上述方法。
更多内容可参考这篇博客的论文解读：
2.扩展目标跟踪
4D雷达点云数量较多，不再适用点目标跟踪，需要引入扩展目标跟踪。
上图左侧是点目标关联，蓝色五角星是参考点，一般为目标中心，通过关联红色的点云进行目标更新。
右侧是扩展目标关联，蓝色方框是目标轮廓，蓝色五角星是参考角点，图中是4个，大目标可以是8个甚至16个。
关联时不再关联所有点云，而是关联聚类后的几何轮廓，使用IOU进行关联，也即计算轮廓的交叠面积。
更新目标时，先使用每个角点的坐标做滤波处理，再更新最终的目标。