一、基本原理
毫米波雷达检测的通常是点目标，没有长宽和航向信息，但可以通过历史轨迹和运动速度估计。
二、历史轨迹估计航向角
如下图记录雷达目标的历史轨迹，则通过直线拟合可以估计航向角。
很明显，轨迹越长，估计的结果越准确；每次目标的位置越精确，估计的结果也越准确。
但由于雷达检测的点每次可能是目标的不同部位，因此目标位置精度可能比较差，此时需要更多的历史轨迹信息来推算。比如下图：
这种情况下，估计的历史轨迹自然更精确。
或者，通过相对的平滑，降低每个位置的波动，提升角度估计的准确性。
如果目标快速转弯，则可能导致估计的航向角误差较大，但考虑到绝大多数情况，目标在短时间（1s内）不太可能出现，因此通常仍旧使用直线拟合。
三、运动速度估计航向角
速度估计航向角原理简单，用以下公式表示
θ = arctan（vy/vx）
由于单帧速度估计存在偏差，容易导致估计误差较大，因此可以考虑使用多帧平均或直线拟合来估计。
如果vy和vx都比较小，同样容易导致估计误差较大，因此高速情况下置信度较高，低速情况下置信度较低。
四、雷达运动时历史轨迹估计航向角
雷达从0时刻开始，运动到3时刻，每次位移表示为e1、e2、e3，目标坐标为t1、t2、t3，相对距离为d1、d2、d3，则有
t1 = e1 + d1
t2 = e1 + e2 + d2
t3 = e1 + e2 + e3 + d3
依次类推，可以得到任意时刻的目标轨迹，再根据上述提到的历史轨迹轨迹估计航向角的方法计算航向角。
这里假设雷达坐标系只有平移变换，未发生角度变换，实际上雷达坐标系存在角度翻转，需要重新计算坐标。
雷达从位置0移动到位置1，角度为θ，坐标系偏转为α，则位置1坐标系下t1相对位置0的坐标为
t1 = d1 + e1‘ r1 = sqrt(e1.x * e1.x + e1.y * e1.y)
cos(θ1) = e1.x / r1 sin(θ1) = e1.y / r1
-e1’.x = r1 * cos(θ1+α1) = e1.x * cos(α1) - e1.y * sin(α1)
-e1’.y = r1 * sin(θ1+α1) = e1.x * sin(α1) + e1.y * cos(α1)
t1.x = d1.x – e1’.x = d1.x + e1.x * cos(α1) - e1.y * sin(α1)
t1.y = d1.y – e1’.y = d1.y + e1.x * sin(α1) + e1.y * cos(α1)
对于位置2，d2不做旋转，e1，e2和d1都需要做一次旋转，同上述计算方法，可得到如下结果。
t2.x = d2.x + e1.x * cos(α1 + α2) – e1.y * sin(α1 + α2) + e2.x * cos(α2) – e2.y * sin(α2)
t2.y = d2.y + e1.x * sin(α1 + α2) + e1.y * cos(α1 + α2) + e2.x * sin(α2) + e2.y * cos(α2)
依次类推，每次目标移动后，都将做一次平移和旋转变换，才能得到最终的目标坐标。
根据计算的历史轨迹，再用最小二乘拟合，便可得到目标的航向角。
五、雷达运动时运动速度估计航向角
雷达随自车运动，认为雷达朝向和自车朝向角相同。
自车运动时，把前车轮转角近似认为朝向角，即图中的θ角。车轮转角通过方向盘转角steering angle和传动比k确定，一般传动比8~15。
θ = steering_angle / k
自车速度e.vx = e.v * cosθ， e.vy = e.v * sinθ
t.vx = e.vx + t.vx, t.vy = e.vy + t.vy
目标朝向α = arctan(t.vy/t.vx)
同样，可以通过多帧平滑或拟合提高准确度。