图解卡尔曼滤波及匹配算法进行多目标跟踪

多目标跟踪可以分为两个子任务：

• 1.预测状态
  在卡尔曼滤波算法中，当前时刻的最优估计由上一时刻的最优估计和当前时刻的观测数据共同决定。
• 2.关联匹配
  不同时刻多目标之间的相互联系，及同一时刻多目标之间的相互区别。

多目标跟踪的难点

状态预测（位置、速度等）的准不准，影响了目标之间能否正确匹配（目标编号）。

多目标跟踪.png

多目标跟踪过程

上图是多目标跟踪的一个例子，有效跟踪范围为x=0到x=200之间，红线表示卡尔曼滤波的初始化及更新阶段，绿线表示预测阶段。

当t=1时，

1. M_(4,7)表示观测数据。这里检测到7个目标。（这里用矩阵表示多个目标，矩阵的每一列表示一个目标，每个目标有4维数据，分别是观测到的物体该时刻的位置和速度 [px,py,vx,vy] ）
2. X_^ 表示当前时刻的最优状态估计。（因为这是第一组数据，因此，用当前时刻的观测数据M_对X_^进行初始化）
3. 同时，对t=1时刻的目标进行依次编号，作为初始化结果。

当t=2时，

1. M_(4,6)表示观测数据。这里检测到6个目标。（上一时刻的3号目标在当前时刻超出检测范围，因此不计入观测数据）
2. 通过上一时刻的最优估计X_^，可以根据指定条件筛选符合条件的目标，得到当前时刻的有效估计。（X_(4,7)为上一时刻的预测，假如判断符合条件的目标为(px>0)，则3号目标不符，去除编号3。因此，有效目标只有6个，因此有效估计X_~为(4,6)）
3. 将有效估计X_~(4,6)与当前时刻的观测数据M_(4,6)进行匹配，得到每个目标在当前时刻的编号，这里可以用匈牙利算法进行目标之间的匹配。
4. 利用卡尔曼滤波算法进行当前时刻的最优状态估计X_^(4,6)。

当t=3时，

1. M_(4,8)表示观测数据。这里检测到8个目标，其中5个是已知的，3个是新增目标。（上一时刻的5号目标在当前时刻超出检测范围，因此不计入观测数据）
2. 通过上一时刻的最优估计X_^，可以根据指定条件筛选符合条件的目标，得到当前时刻的有效估计。（X_(4,6)为上一时刻的预测，假如判断符合条件的目标为(px>0)，则5号目标不符，去除编号5。因此，有效目标只有5个，因此有效估计X_~为(4,5)）
3. 将有效估计X_~(4,5)与当前时刻的观测数据M_(4,8)进行匹配，得到每个目标在当前时刻的编号。
4. 对于匹配数据，利用卡尔曼滤波算法更新得到当前时刻的最优状态估计X_^(4,5)；未匹配数据有3个，即新增的3个目标。对于这3个目标，用观测数据对其进行初始化，并编号。
5. 将两部分数据相结合，得到当前时刻的最优状态估计X_^(4,8)。

当t=4时，

1. M_(4,6)表示观测数据。这里检测到6个目标。（上一时刻的2号和7号目标在当前时刻超出检测范围，因此不计入观测数据）
2. 通过上一时刻的最优估计X_^，可以根据指定条件筛选符合条件的目标，得到当前时刻的有效估计。（X_(4,8)为上一时刻的预测，假如判断符合条件的目标为(px>0)，则2号和7号目标不符，去除编号2和编号7。因此，有效目标只有6个，因此有效估计X_~为(4,6)）
3. 将有效估计X_~(4,6)与当前时刻的观测数据M_(4,6)进行匹配，得到每个目标在当前时刻的编号。
4. 利用卡尔曼滤波算法进行当前时刻的最优状态估计X_^(4,6)。

当t=5时，

1. M_(4,3)表示观测数据。这里检测到3个目标。（上一时刻的1号、4号和6号目标在当前时刻超出检测范围，因此不计入观测数据）
2. 通过上一时刻的最优估计X_^，可以根据指定条件筛选符合条件的目标，得到当前时刻的有效估计。（X_(4,6)为上一时刻的预测，假如判断符合条件的目标为(px>0)，则1号、4号和6号目标不符，去除编号1、4、6。因此，有效目标只有3个，因此有效估计X_~为(4,3)）
3. 将有效估计X_~(4,3)与当前时刻的观测数据M_(4,3)进行匹配，得到每个目标在当前时刻的编号。
4. 利用卡尔曼滤波算法进行当前时刻的最优状态估计X_^(4,3)。

当t=6时，

1. M_(4,5)表示观测数据。这里检测到5个目标，其中3个是已知的，2个是新增目标。
2. 通过上一时刻的最优估计X_^，可以根据指定条件筛选符合条件的目标，得到当前时刻的有效估计。（这里上一时刻的3个目标都为有效目标）
3. 将有效估计X_~(4,3)与当前时刻的观测数据M_(4,5)进行匹配，得到每个目标在当前时刻的编号。
4. 对于匹配数据，利用卡尔曼滤波算法更新得到当前时刻的最优状态估计X_^(4,3)；未匹配数据有2个，即新增的2个目标。对于这2个目标，用观测数据对其进行初始化，并编号。
5. 将两部分数据相结合，得到当前时刻的最优状态估计X_^(4,5)。

由上述分析可知，卡尔曼滤波算法对多目标的状态进行估计，匈牙利算法对多目标进行匹配，实现多目标跟踪。

卡尔曼滤波算法中的公式

参考https://www.pianshen.com/article/9795849360/
公式中 A和H为转移矩阵

kalman.jpg