DeepSORT（目标跟踪算法）中的卡尔曼滤波 - 看了就会的状态转移矩阵

flyfish

https://github.com/shaoshengsong/DeepSORT

假设位置的变化由速度线性决定

在Kalman滤波器中，我们通常假设目标的运动是线性的。即，目标的下一时刻的位置可以由当前的位置和速度决定。具体来说，对于状态向量 $\mathbf{z}$：
$$\mathbf{z}=[x,y,a,h,vx,vy,va,vh{]}^T$$
这里：

• $x$ 和 $y$ 是位置坐标，
• $a$ 是纵横比（宽度/高度），
• $h$ 是高度，
• $vx$ 和 $vy$ 是位置的速度，
• $va$ 是纵横比的变化率，
• $vh$ 是高度的变化率。
  下一时刻的状态向量 ${\mathbf{z}}^{\prime }$ 可以由以下公式线性估计：
  $$\begin{array}{rl}x(t+1)& =x(t)+vx(t)\cdot dt\\ y(t+1)& =y(t)+vy(t)\cdot dt\\ a(t+1)& =a(t)+va(t)\cdot dt\\ h(t+1)& =h(t)+vh(t)\cdot dt\\ vx(t+1)& =vx(t)\\ vy(t+1)& =vy(t)\\ va(t+1)& =va(t)\\ vh(t+1)& =vh(t)\end{array}$$

状态转移矩阵（F）的例子

为了实现上述状态转移，我们构造一个状态转移矩阵 $\mathbf{F}$。对于4维位置（x, y, a, h）和4维速度（vx, vy, va, vh），$\mathbf{F}$ 是一个8x8的矩阵，用于将当前状态 $\mathbf{z}(t)$ 转换为下一时刻的状态 $\mathbf{z}(t+1)$。
$$\mathbf{F}=\left[\begin{array}{cccccccc}1& 0& 0& 0& dt& 0& 0& 0\\ 0& 1& 0& 0& 0& dt& 0& 0\\ 0& 0& 1& 0& 0& 0& dt& 0\\ 0& 0& 0& 1& 0& 0& 0& dt\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

具体例子

假设当前状态向量 $\mathbf{z}(t)$ 为：
$$\mathbf{z}(t)=[2,3,1.5,4,0.1,0.2,0.01,0.05{]}^T$$
即：

• 位置 $x=2$, $y=3$,
• 纵横比 $a=1.5$,
• 高度 $h=4$,
• 速度 $vx=0.1$, $vy=0.2$,
• 纵横比变化率 $va=0.01$,
• 高度变化率 $vh=0.05$。
  时间步长 $dt=1$。应用状态转移矩阵 $\mathbf{F}$，计算下一时刻的状态向量 $\mathbf{z}(t+1)$：
  $\mathbf{z}(t+1)=\mathbf{F}\cdot \mathbf{z}(t)$
  计算结果如下：
  $$\mathbf{z}(t+1)=\left[\begin{array}{cccccccc}1& 0& 0& 0& 1& 0& 0& 0\\ 0& 1& 0& 0& 0& 1& 0& 0\\ 0& 0& 1& 0& 0& 0& 1& 0\\ 0& 0& 0& 1& 0& 0& 0& 1\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}2\\ 3\\ 1.5\\ 4\\ 0.1\\ 0.2\\ 0.01\\ 0.05\end{array}\right]=\left[\begin{array}{c}2+0.1\\ 3+0.2\\ 1.5+0.01\\ 4+0.05\\ 0.1\\ 0.2\\ 0.01\\ 0.05\end{array}\right]=\left[\begin{array}{c}2.1\\ 3.2\\ 1.51\\ 4.05\\ 0.1\\ 0.2\\ 0.01\\ 0.05\end{array}\right]$$
  这表明下一时刻的位置和尺寸根据当前速度进行了更新，而速度保持不变。这样，Kalman滤波器可以基于这个预测和实际观测值进行融合，从而得到更准确的目标状态估计。

class KalmanFilter(object):

    def __init__(self):
        ndim, dt = 4, 1.

        self._motion_mat = np.eye(2 * ndim, 2 * ndim)
        for i in range(ndim):
            self._motion_mat[i, ndim + i] = dt
            
        self._update_mat = np.eye(ndim, 2 * ndim)
        
        self._std_weight_position = 1. / 20
        self._std_weight_velocity = 1. / 160

• 构造方法:

def __init__(self):

这是 Python 中的构造方法，当创建类的新实例时会调用它。它初始化对象的属性并设置卡尔曼滤波器矩阵。在 init 方法中，执行以下操作：

ndim, dt = 4, 1.

ndim 表示状态空间位置部分的维数（例如 x, y, a, h）。dt 是时间步长（时间增量），通常设置为 1，假设每个预测步长对应一个时间单位。
创建卡尔曼滤波器模型矩阵:

self._motion_mat = np.eye(2 * ndim, 2 * ndim)
for i in range(ndim):
    self._motion_mat[i, ndim + i] = dt
self._update_mat = np.eye(ndim, 2 * ndim)

self._motion_mat 是运动模型矩阵，初始化为单位矩阵。它表示状态如何随时间变化。
self._update_mat 是观测模型矩阵，也初始化为单位矩阵，将状态映射到观测测量。
循环部分：

for i in range(ndim):
    self._motion_mat[i, ndim + i] = dt

在运动矩阵中设置时间步长关系。例如，对于 x（位置）和 vx（速度）：$\text{motion\_mat}[0,4]=dt\text{（x 更新为 vx * dt）}$

- self._std_weight_position = 1. / 20
- self._std_weight_velocity = 1. / 160

• 这些值是用于位置和速度标准差的权重，它们控制模型中的不确定性：
• 1 / 20: 表示位置的标准差假设为测量范围的 $\frac{1}{20}$。
• 1 / 160: 类似地，速度的标准差假设为测量范围的 $\frac{1}{160}$。

这些值是根据经验选择的，意味着它们是基于实际结果和微调选择的，而不是从严格的数学公式推导出来的。目的是在观测测量（边界框坐标）的变化敏感性与平滑噪声和提供稳定跟踪之间取得平衡。

初始化方法

def __init__(self):
    ndim, dt = 4, 1.

    # 创建卡尔曼滤波器模型矩阵。
    self._motion_mat = np.eye(2 * ndim, 2 * ndim)
    for i in range(ndim):
        self._motion_mat[i, ndim + i] = dt
    self._update_mat = np.eye(ndim, 2 * ndim)

    # 运动和观测不确定性相对于当前状态估计的选择。这些权重控制模型中的不确定性量。
    self._std_weight_position = 1. / 20
    self._std_weight_velocity = 1. / 160

初始化卡尔曼滤波器的状态转移矩阵 (self._motion_mat) 和观测矩阵 (self._update_mat)，以及位置和速度标准差的权重，用于控制模型的不确定性。

用线性方程来描述对象的位置和速度随时间的变化。在这里，假设位置的变化由速度线性决定，具体公式为：
$$\begin{array}{rl}x(t+1)& =x(t)+vx(t)\cdot dt\\ y(t+1)& =y(t)+vy(t)\cdot dt\\ a(t+1)& =a(t)+va(t)\cdot dt\\ h(t+1)& =h(t)+vh(t)\cdot dt\end{array}$$

_motion_mat（运动矩阵）

运动矩阵（$\mathbf{F}$）将上述线性关系表达为矩阵形式。具体如下：

self._motion_mat = np.eye(2 * ndim, 2 * ndim)
for i in range(ndim):
    self._motion_mat[i, ndim + i] = dt

np.eye(2 * ndim, 2 * ndim) 生成一个8x8的单位矩阵，然后通过循环将时间步长 dt 添加到矩阵的前4行后4列，从而实现线性运动关系。最终的矩阵如下：
$$\mathbf{F}=\left[\begin{array}{cccccccc}1& 0& 0& 0& dt& 0& 0& 0\\ 0& 1& 0& 0& 0& dt& 0& 0\\ 0& 0& 1& 0& 0& 0& dt& 0\\ 0& 0& 0& 1& 0& 0& 0& dt\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$
这个矩阵表示的位置和速度之间的线性关系。每次状态更新时，位置部分会加上速度部分乘以时间步长 dt。

_update_mat（更新矩阵）

更新矩阵（$\mathbf{H}$）用于将观测结果转换为状态向量。在这个例子中，观测结果只包含位置和大小（不包括速度），所以它是一个4x8的矩阵：

self._update_mat = np.eye(ndim, 2 * ndim)

生成的矩阵如下：
$$\mathbf{H}=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\end{array}\right]$$
这个矩阵表示我们只更新状态向量中的位置和大小部分，而不更新速度部分。

假设我们有一个在二维平面上运动的物体，其状态包括位置和速度。状态向量可以表示为：

$$x=\left[\begin{array}{c}x\\ y\\ vx\\ vy\end{array}\right]$$

其中：

• $x$ 和 $y$ 是物体的位置。
• $vx$ 和 $vy$ 是物体的速度。

状态转移矩阵 $F$

假设物体的运动是匀速直线运动，即位置随时间线性变化，速度保持不变。状态转移矩阵 $F$ 可以表示为：
$$\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
其中$\Delta t$ 是时间间隔。

控制矩阵 ( B ) 和控制输入 ( u )

假设没有外部控制输入（即 ( u = 0 )），则控制矩阵 ( B ) 和控制输入 ( u ) 可以忽略。

过程噪声 ( w )

过程噪声 $w$ 描述系统的随机扰动，假设为零均值高斯噪声，协方差为$Q$。

状态转移方程

在没有控制输入的情况下，状态转移方程简化为：
$$x_k=F{x}_{k-1}+w_k$$
具体为：
$$\left[\begin{array}{c}{x}_k\\ y_k\\ v{x}_k\\ v{y}_k\end{array}\right]=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ v{x}_{k-1}\\ v{y}_{k-1}\end{array}\right]+\left[\begin{array}{c}{w}_x\\ w_y\\ w_{vx}\\ w_{vy}\end{array}\right]$$

示例计算

假设在时刻 ( k-1 )，物体的状态为：
$$x_{k-1}=\left[\begin{array}{c}10\\ 5\\ 1\\ 2\end{array}\right]$$
表示物体的位置为 (10, 5)，速度为 (1, 2)。假设时间间隔 $\Delta t=1$，状态转移方程可以计算出下一时刻 ( k ) 的状态：
$$x_k=\left[\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}10\\ 5\\ 1\\ 2\end{array}\right]+\left[\begin{array}{c}{w}_x\\ w_y\\ w_{vx}\\ w_{vy}\end{array}\right]$$
忽略过程噪声（假设 $w_k$ = 0 )）：
$$x_k=\left[\begin{array}{c}10+1\\ 5+2\\ 1\\ 2\end{array}\right]=\left[\begin{array}{c}11\\ 7\\ 1\\ 2\end{array}\right]$$

展示矩阵乘法的过程
$$\left[\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}10\\ 5\\ 1\\ 2\end{array}\right]=\left[\begin{array}{c}1\cdot 10+0\cdot 5+1\cdot 1+0\cdot 2\\ 0\cdot 10+1\cdot 5+0\cdot 1+1\cdot 2\\ 0\cdot 10+0\cdot 5+1\cdot 1+0\cdot 2\\ 0\cdot 10+0\cdot 5+0\cdot 1+1\cdot 2\end{array}\right]=\left[\begin{array}{c}10+1\\ 5+2\\ 1\\ 2\end{array}\right]=\left[\begin{array}{c}11\\ 7\\ 1\\ 2\end{array}\right]$$
因此，下一时刻 ( k ) 的状态估计为位置 (11, 7)，速度保持不变 (1, 2)。

状态转移矩阵（State Transition Matrix）$\mathbf{F}$的构造

状态转移矩阵（State Transition Matrix）$\mathbf{F}$的构造基于系统的运动模型。它描述了系统状态在时间上的演化关系。在这个具体的例子中，我们使用了一个简单的线性运动模型来描述目标的位置和速度的变化。下面是详细的解释。

线性运动模型

假设系统状态向量包含位置和速度，例如：
$$\mathbf{z}=\left[\begin{array}{c}x\\ y\\ vx\\ vy\end{array}\right]$$
其中：

• $x$ 和 $y$ 是位置，
• $vx$ 和 $vy$ 是速度。
  我们使用离散时间步长 $\Delta t$ 来更新状态向量。线性运动模型假设在每个时间步长 $\Delta t$ 内，位置的变化是由速度决定的，并且速度在时间步长内保持不变。因此，下一时刻的状态可以表示为：
  $$\begin{array}{rl}x(t+\Delta t)& =x(t)+vx(t)\cdot \Delta t\\ y(t+\Delta t)& =y(t)+vy(t)\cdot \Delta t\\ vx(t+\Delta t)& =vx(t)\\ vy(t+\Delta t)& =vy(t)\end{array}$$

状态转移矩阵的构造

为了用矩阵形式表示上述线性运动模型，我们引入状态转移矩阵 $\mathbf{F}$。状态转移矩阵用于将当前状态向量转换为下一时刻的状态向量：
$\mathbf{z}(t+\Delta t)=\mathbf{F}\cdot \mathbf{z}(t)$
对于上述模型，状态转移矩阵 $\mathbf{F}$ 的构造如下：
$$\mathbf{F}=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
解释每一部分：

1. 位置更新部分：

• $x(t+\Delta t)=x(t)+vx(t)\cdot \Delta t$这一行的矩阵表示为：$1\cdot x+0\cdot y+\Delta t\cdot vx+0\cdot vy$
• $y(t+\Delta t)=y(t)+vy(t)\cdot \Delta t$这一行的矩阵表示为：$0\cdot x+1\cdot y+0\cdot vx+\Delta t\cdot vy$

2. 速度保持部分：

• $vx(t+\Delta t)=vx(t)$这一行的矩阵表示为：$0\cdot x+0\cdot y+1\cdot vx+0\cdot vy$
• $vy(t+\Delta t)=vy(t)$这一行的矩阵表示为：$0\cdot x+0\cdot y+0\cdot vx+1\cdot vy$
  将这些方程综合起来，就得到了状态转移矩阵 $\mathbf{F}$：
  $$\mathbf{F}=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

C++ 版本的代码分析

https://gitee.com/ascend/ascend_community_projects/tree/master/deepsort在华为昇腾的代码示例中，关于多目标跟踪使用 https://github.com/shaoshengsong/DeepSORT 这份C++代码。

DeepSORT目标跟踪后处理插件基于MindXSDK开发，在晟腾芯片上进行目标检测和跟踪，可以对行人进行画框和编号，将检测结果可视化并保存。项目主要流程为：通过live555服务器进行拉流输入视频，然后进行视频解码将264格式的视频解码为YUV格式的图片，图片缩放后经过模型推理进行行人识别，识别结果经过FairMOT后处理后得到识别框，对识别框进行跟踪并编号，用编号覆盖原有的类别信息，再将识别框和类别信息分别转绘到图片上，最后将图片编码成视频进行输出。

C++版本 参考了 Python版，所以可以两边的代码都 可以分析。