说明
本篇博文作为目标跟踪系列博文的第2篇，对目标跟踪做仿真实践。具体地，通过预设不同的目标并为之赋予不同的雷达测量参数(距离、速度、角度等)，随后将预设目标信息送入后端的跟踪算法，查看目标跟踪情况。
在阅读本文之前，建议读者先看看本系列的第1篇博文[1] ：（雷达数据处理中的)跟踪算法(1) --- 整体&目录-CSDN博客 ，本文所做的仿真相对比较简单(只做了单目标仿真)，不过可以作为一个模板，读者可以在本文所提供的代码等基础上做更深入的研究。
Blog
20240724 博文第一次撰写
目录
[2.1 滤波模块的目标运动模型设计与参数初始化](#2.1 滤波模块的目标运动模型设计与参数初始化)
[2.2 关联模块细节设计](#2.2 关联模块细节设计)
[2.3 航迹管理模块设计](#2.3 航迹管理模块设计)
[2.4 本章小结](#2.4 本章小结)
[3.1 一维匀加速运动模型下的目标跟踪结果](#3.1 一维匀加速运动模型下的目标跟踪结果)
[3.2 二维匀速运动模型下的目标跟踪结果](#3.2 二维匀速运动模型下的目标跟踪结果)
[3.3 本章小结](#3.3 本章小结)
一、仿真方案设计
本文我预设了并给出了单目标的一维匀加速运动、二维匀速运动两种目标运动模型(方案)的仿真结果。【不过在所提供的代码中支持：一维匀速运动、一维匀加速运动、二维匀速运动、二维匀加速运动四种情况，读者可以基于代码很方便地自行设计目标参数】。本文给出的示例下，具体的目标参数设计列表如下：
表1.1 一维匀加速运动目标参数设计(方案1)
|--------|-------------------------------------------------|
| 参数 | 值 |
| 仿真总帧数 | 100（认为目标从第一帧便开始被探测到，且在100帧内都没有丢失） |
| 帧周期 | 50ms |
| 目标数量 | 1 |
| 目标运动模型 | 一维匀加速运动 |
| 目标运动参数 | 起始距离20m，初始速度30km/h，加速度值5m/s^2 |
| 所加测量噪声 | 测距精度: 0.2m; 测速精度: 0.1m/s (测量噪声的均值为0，标准差为测量精度) |
| 备注 | 认为目标速度为正表示目标远离，目标速度为负表示目标靠近。本次仿真中没有考虑目标的角度。 |
表1.2 二维匀速运动目标参数设计(方案2)
|--------|--------------------------------------------------------------------------------------------------------------|
| 仿真总帧数 | 100（认为目标从第一帧便开始被探测到，且在100帧内都没有丢失） |
| 帧周期 | 50ms |
| 目标数量 | 1 |
| 目标运动模型 | 二维匀速运动 |
| 目标运动参数 | 起始距离：Rx = 20m; Ry = 0m; 初始速度Vx = 30km/h; Vy = 10km/h; |
| 所加测量噪声 | 测距精度: 0.2m; 测速精度: 0.1m/s (与实测有些出入的地方是，为方便目标生成，我将噪声单独加到了两个方向上，而不是加到距离和速度的矢量和方向上。所以两个方向测量噪声的均值都为0，标准差为各自的测量精度) |
| 备注 | 认为目标速度为正表示目标远离，目标速度为负表示目标靠近。本次仿真中没有考虑目标的角度。 |
在前述目标参数设计下，所生成的目标如下所示：
图1.1 一维匀加速运动模型下所生成的目标
（从左至右分别为：理想情况、加噪声后、加噪声前后对比）
图1.2 图1.1右图局部放大后
图1.3 二维匀速运动模型下所生成的目标
（从左至右分别为：理想情况、加噪声后、加噪声前后对比）
图1.4 图1.3右图局部放大后
在后续的目标跟踪实践中，是将加噪声后的目标输入给到跟踪模块完成跟踪。
二、跟踪算法细节设计
关于整个跟踪模块的处理流程，我在本系列的第一篇博文**[1]** 中有过比较详细的描述，读者可以阅读参考。本章只贴出跟踪模块的整体流程图(沿用上一篇博文)，并针对跟踪部分的一些细节做出说明。
图2.1 跟踪模块算法流程图
(关于该流程的说明，请参考本系列第一篇博文[1])
2.1 滤波模块的目标运动模型设计与参数初始化
在滤波模块中，我们需要拟定目标的运动模型，并基于运动模型做参数初始化(如：目标的加速度值、状态转移矩阵、量测矩阵以及三个协方差矩阵)。针对前述两个目标运动模型，所设计目标运动模型和初始化参数值如下：
表2.1 一维匀加速运动目标滤波参数设计列表
|---------------------------|----------------------------------------------------|
| 参数 | 值 |
| 所选用运动模型 | 一维匀加速运动模型（读者可以尝试用匀速运动模型去处理匀加速运动目标） |
| 量测矩阵初始化 | [1,0,0;0,1,0;0,0,1] |
| 状态转移矩阵初始化 | [1,dt,0.5*dt^2;0,1,dt;0,0,1]， dt为帧周期(50ms)。 |
| 量测过程的噪声协方差矩阵初始化 (用测量精度构建) | diag[距离测量精度^2,速度测量精度^2,速度测量精度^2]，diag为构造对角矩阵。 |
| 预测过程的噪声协方差矩阵初始化 | diag[0,0,0] |
| 状态预测的协方差矩阵初始化 | 同初始化的量测过程的噪声协方差矩阵 |
| 加速度值初始化 | 利用至少前四帧的数据取均值 |
【补充 ：关于加速度值初始化的详细说明】
不难理解的是：加速度值对于雷达而言是无法在单帧之下直接测量得到的，我们需要至少两帧的速度测量结果V1、V2，结合帧周期dT才能得到：
(2-1)
所以在初始化加速度值时，我们还需要有一个关联的操作：我们需要把前后帧的同一个目标关联起来！本文的航迹管理部分，我在各个目标新出现的第一帧就为其新建了一条航迹(不过将新建的航迹标识为"Notconfirmed")，所以这里为初始化加速度的关联是在正常的航迹管理流程中的，不需要单独做。
此外，从实践中发现 ，由于帧周期很小，相邻帧的速度变化量很小，如果我们只基于前两帧的测速结果求解加速度值，此时所加的测速噪声可能会严重影响加速度值的准确性！ 所以在加速度值初始化中，我以前四帧所得的加速度均值来初始化该加速度值，部分实现的代码如下：
图2.2 加速度值初始化部分代码
obj.state对应的是目标状态，一维匀加速运动模型下它是一个1*3大小的向量：[距离，速度，加速度]，在第一帧下新建航迹时，加速度值设置为0，如果第2帧我们关联上了该目标，则其加速度值为：(本帧的测速结果target.V -- 上一帧的速度值obj.state(2))/帧周期，此外，如果这条航迹的age < 4，会对加速度值做进一步的更新（需要注意的是：此时所用的obj.state(2)是滤波后的速度，并不是上一帧的测量结果了，所以可能会有累积误差...）。
从仿真的结果来看，前述操作后，加速度值会更加准确，(特别是测速误差较大的情况下)。读者可以基于代码，通过改变测速精度等变量实践看看，读者也可以尝试更好的加速度值初始化方法。
表2.2 二维匀速运动目标滤波参数设计列表
|-----------------|---------------------------------------------------|
| 参数 | 值 |
| 所选用运动模型 | 二维匀速运动模型 |
| 量测矩阵初始化 | [1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1] |
| 状态转移矩阵初始化 | [1,dt,0,0;0,1,0,0;0,0,1,dt;0,0,0,1] |
| 量测过程的噪声协方差矩阵初始化 | diag[距离测量精度^2,速度测量精度^2, 距离测量精度^2，速度测量精度^2，] |
| 预测过程的噪声协方差矩阵初始化 | diag[0,0,0,0] |
| 状态预测的协方差矩阵初始化 | 同初始化的量测过程的噪声协方差矩阵 |
注：前述各类协方差矩阵的初始化应该不是最优的，关于这几个矩阵的意义，及其初始化的建议，读者可以参考本系列的第一篇博文[1]中的相关内容。读者也可以设计更为合理的初始化矩阵。
2.2 关联模块细节设计
关联是指对现有航迹和当前帧下的目标测量结果进行关联。本次仿真所采用的方法是经典的最近邻法。以距离和速度这两个参数来评估目标与航迹之间的"统计距离"，找到距离的最近完成关联。
图2.3 关联模块部分代码
关联部分的实现思路是：构建一个大小为航迹数量(nTracks) * 目标数量（nDetection）的统计距离矩阵cost,并预设一个阈值costOfNonAssignment，我们基于阈值和cost矩阵完成航迹和目标之间的关联Assign。
统计距离cost是通过计算相对距离cost_dist和相对特征cost_feature，并给它们不同的权重值相加得到的 。相对距离就是计算几何距离dR，相对特征这里我只选用了速度这一特征，(在更复杂或更准确的关联算法中，我们还可以引入如RCS值、功率值等特征进行求解)。
阈值costOfNonAssignment的存在是为了防止统计距离相隔较大的航迹和目标被关联上：比如最简单的情况，现在只有一条航迹和一个目标，但是它们相隔很远，不可能是同一个目标，此时其统计距离远大于我们预设的阈值，我们不能将之关联起来（这在前一篇博文中有相应的描述）。
关联Assign的实现流程如下：
图2.4 关联函数Assign实现流程
关联后，我们会得到：关联上的航迹和目标索引assignments、没有被关联上的航迹索引unassignedTracks、没有被关联上的目标索引unassignedDetetions。随后我们基于关联的结果分别进行航迹的维护、航迹的删除和创建等工作。
2.3 航迹管理模块设计
航迹管理模块完成航迹的新建、维护和删除等任务，本次仿真下预设的航迹管理参数及其用途列表如下：
表2.3 航迹管理模块相关参数及其用途说明列表
|---------------------------|----------------------------------------------------------------------------------------------------|
| 参数 | 用途说明 |
| id | 用于给每条航迹分配一个特定的ID号 |
| tracksNum_MAX | 最大支持的航迹数量(考虑到内存和计算资源的限制，一般车载雷达都有最大可跟踪目标数量的限制) |
| ID_Used | 是一个大小为1* tracksNum_MAX的0/1数组，在航迹管理中，ID号只能在1->tracksNum_MAX范围内选取，如果某个索引处的ID号被使用，则ID_Used该位置处的值为1。 |
| traj_rec | 用以存储该航迹的历史位置信息，该信息可以用在GUI界面上显示，以及可以用作debug。(不过在实际的车载产品应用中，这个参数是可以被抛弃的) |
| appear_frame | 航迹被建立时的帧ID（或者说这条航迹对应目标初次出现的帧ID） |
| obj_feature | 用以存储航迹对应的目标的特征，比如可以是速度、RCS、功率值等，在本次仿真中，我只加入了速度。 （该值在做关联时会被用到） |
| age | 航迹存在的帧数(从appear_frame开始计算，每过一帧该值增加1) |
| state | 航迹现阶段的状态："Notconfirmed"、"normal"、"lost"、"Noise"可选，关于这几种状态之间的转换，读者可以参考本系列的第一篇博文和后文图2.5 |
| totalVisibleCount | 航迹被关联上的总帧数 |
| consecutiveInvisibleCount | 航迹连续没有被关联上的帧数 |
| TTC | 碰撞时间。(后续用于评价航迹优先级) |
| motionModel | 该航迹所预设的运动模型 |
| motionDim | 该航迹所预设的目标运动维度 |
图2.5 航迹不同状态之间的转换关系
【关于航迹管理部分的一些细节补充】
1. 航迹的新建、转换关系以及删除
在车载毫米波雷达领域，考虑到输入给到跟踪部分的目标是经过了CFAR检测、点云聚类之后的结果，所以整体上目标的可信度是很高的(目标是干扰/噪声造成的概率很低，不过也存在是干扰的可能)，所以在本文的仿真实践中，每一帧下没有被关联上的新目标，我会即刻给它新建一条航迹，不过此时的航迹被标识为"Notconfirmed"，其输出的结果并不会被送入后端的应用模块。我们定义Visibility参数为：
（2-2）
并设置参数ageThreshold等于4，visibilityThrehold等于0.5，并约定 ，当航迹的参数满足：
ages <= ageThreshold && visibility < visibilityThrehold
时，认为航迹为"Noise",该航迹对应的目标可能是干扰所产生的，被标识为"Noise"的航迹会在随后被删除。
当航迹的参数满足：
ages > ageThreshold
时，认为航迹为"Normal",该航迹对应的目标是正常目标，被标识为"Normal"的航迹其结果会被送入后端的应用模块。
此外，我们预设参数invisibleForTooLong等于6，当航迹的参数满足：
consecutiveInvisibleCount >= invisibleForTooLong
时，认为航迹为"Lost"，该航迹对应的目标已经丢失了，被标识为"Normal"的航迹其结果会在随后被删除。
2. 航迹新建的补充说明
在前文中，我们预设了tracksNum_MAX这个参数，因为考虑到在实际应用中，内存和计算资源都是有限的，我们无法无限制地增加航迹。本文提供的代码中，我将该值预设为64，当然，因为本文所设计的仿真实验中只有一个目标，所以航迹数不会超过该预设的最大值，不过在代码中我还是对可能超过64的情况(航迹数量溢出)做了讨论。
我在代码中基于碰撞时间TTC、距离等参数为航迹设计了一个优先级的评价标准(具体见图2.6)，当现有航迹数量达到预设的最大航迹数时，如果新目标的优先级高于现有航迹中的最低优先级，则将现有最低优先级的航迹删除，并将其id号赋值给新目标对应的航迹，如果新目标的优先级低于现有航迹中的最低优先级，则不为该目标新建航迹。这部分的算法处理流程如下图所示：
图2.6 新建航迹的算法流程&航迹优先级评价标准
3. 备注
本文(本次仿真)给出的航迹管理参数和方法，只是一个比较简单的尝试和参考，读者可以在本文的基础上做更多优化，跟踪效果的好坏除依赖于滤波算法外，航迹管理也很重要，航迹管理参数的设计需要结合实际的应用情景、做大量的实测实验迭代优化。
2.4 本章小结
本章在本系列第一篇博文[1]的基础上，对整个跟踪模块的实现细节(滤波的初始化、关联算法的实现细节、航迹管理的设计等)进行了说明。后续的仿真将在本章的指导下进行实践。
三、仿真结果与讨论
3.1 一维匀加速运动模型下的目标跟踪结果
图3.1 一维匀加速运动模型下目标跟踪结果(1)
图3.2 一维匀加速运动模型下目标跟踪结果(2)
结合目标生成的结果，可以看到，跟踪的效果基本符合预期。
3.2 二维匀速运动模型下的目标跟踪结果
图3.3 二维匀速运动模型下的目标跟踪结果(1)
图3.4 二维匀速运动模型下的目标跟踪结果(2)
结合目标生成的结果，可以看到，跟踪的效果基本符合预期。
3.3 本章小结
本章在前文的基础上，编写了跟踪模块的一整套代码，并将所生成(加噪声后的)目标送入跟踪模块进行了跟踪实践。跟踪的结果符合预期，验证了整套处理流程的正确性。
四、总结
本文对跟踪算法做了仿真实践。首先对仿真方案做了介绍，随后在[1]的基础上对跟踪算法各模块的设计细节做了补充说明，在前述工作的基础上，仿真给出了一维匀加速运动、二维匀速运动两种目标运动模型下的目标跟踪结果，结果符合预期。本文的工作算法是跟踪算法的一个粗浅的仿真尝试，读者可以在本文的基础上设计更复杂的目标参数、场景参数等更深入地理解跟踪算法，并迭代优化该算法。
五、参考资料
[1] （雷达数据处理中的)跟踪算法(1) --- 整体&目录-CSDN博客
六、参考代码
本跟踪系列博文所有相关的数据和代码我一并打包在了第一篇博文的链接中，读者可以去第一篇博文[1]的文末查收。