多目标跟踪算法理论基础
SORT算法概述
概述：SORT是一种多目标跟踪算法，可以有效地关联目标，并提升跟踪的实时性。SORT的核心主要是卡尔曼滤波和匈牙利算法的结合版，可以达到较好的跟踪效果。在当时，追踪速度达到了260HZ，相比其他方法速度提升了20倍。
Deepsort的前身是sort算法，sort算法的核心是卡尔曼滤波算法和匈牙利算法。
卡尔曼滤波算法作用：该算法的主要作用就是当前的一系列运动变量去预测下一时刻的运动变量，但是第一次的检测结果用来初始化卡尔曼滤波的运动变量。
匈牙利算法
的作用：简单来讲就是解决分配问题，就是把一群
检测框
和卡尔曼
预测的框
做分配，让卡尔曼预测的框找到和自己最匹配的检测框，达到追踪的效果。
SORT算法流程
sort工作流程如下图所示：
- 将第一帧检测到的结果创建其对应的Tracks。将卡尔曼滤波的运动变量初始化，通过卡尔曼滤波预测其对应的框框。
- 将该帧目标检测的框框和上一帧通过Tracks预测的框框一一进行IOU匹配，再通过IOU匹配的结果计算其代价矩阵（cost matrix，其计算方式是1-IOU）。
- 将（2）中得到的所有的代价矩阵作为匈牙利算法的输入，得到线性的匹配的结果，这时候我们得到的结果有三种，第一种是Tracks失配（Unmatched Tracks），我们直接将失配的Tracks删除；第二种是Detections失配（Unmatched Detections），我们将这样的Detections初始化为一个新的Tracks（new Tracks）；第三种是检测框和预测的框框配对成功，这说明我们前一帧和后一帧追踪成功，将其对应的Detections通过卡尔曼滤波更新其对应的Tracks变量。
匈牙利算法Hungarian Algorithm
匈牙利算法是一种在多项式时间内求解任务分配问题的组合优化算法。最初用来解决的是有权二部图中的最小匹配
Minimum-Weight Bipartite Matching问题
在多目标跟踪(Multi-objecttracking,MOT)中，匈牙利算法主要被用于
数据关联。即在连续的帧之间确定每个目标的ID。在实时的场景中，例如，视频流处理，我们需要在每一帧中识别和跟踪多个目标。这就涉及到一个关键的问题，
即如何从一帧到下一帧保持对目标的追踪。匈牙利算法在这里就可以派上用场。可以构建一个
成本矩阵，每个单元格的成本是这两个目标之间的距离（例如，中心点之间的欧氏距离，或者边界框之间的loU距离等）。然后，目标就是找到一个配对，使得总的配对成本最小，这就是一个典型的最优分配问题，可以用匈牙利算法来求解。f ( S ) = ∑ ( u , v ) ∈ S w u v f(\mathcal{S})=\sum_{(u, v) \in \mathcal{S}} w_{u v}f(S)=(u,v)∈S∑wuv
标注的Hungarian Algorithm算法对于的代价矩阵是一个nxn的方阵。下面是一个匈牙利匹配算法的例子
- 找到每一行的最小值然后减去每一行的最小值。（保证每一行最少有一个0）
- 之后按照相同的方法找到每一列的最小值。（保证每一列至少有一个0）
3. 之后我们进行循环的操作，每一次的循环操作包括下面的三个步骤组成。
- 用尽量少的线覆盖住矩阵中所以的0元素。(Cover all the zeros with aminimum number of lines)
- 判断是否需要终止循环Decide whether to stop （用n条线可以覆盖住所有的0元素则终止循环）
- 对矩阵中的0元素最变化产生更多的0元素- （在没有被覆盖的元素上寻找最下的元素记作k）
- 没有被线覆盖住的元素都减去k 
- 交叉位置处的元素都加上k
之后按照这个步骤进行下一轮的循环知道整个循环完毕进行退出的时候结束。
之后就可以完成匹配操作了
采用同样的算法思路Hungarian Algorithm算法也可以解决Minimum-Weight Bipartite Matching问题，只需要对权重先进行一步取反的操作即可。
卡尔曼滤波Kalman Filter
Optimal estimation algorithm：是一种最优的估计算法。
卡尔曼滤波器
的主要步骤可以分为
两个
阶段：
预测阶段
和
更新阶段
。
- 预测阶段：这一阶段根据系统的动态模型对系统的当前状态进行预测并预测系统状态的
协方差（表示预测的不确定性）。预测的结果将被用作下一步更新阶段的输入。 - 更新阶段：在这一阶段，滤波器使用新的观测数据来修正预测阶段的预测结果。这一步涉及到计算
卡尔曼增益它决定了我们应该如何在预测和实际测量之间取平衡。然后，滤波器会更新系统状态的估计和状态的协方差。
卡尔曼滤波公式
卡尔曼提出的递推最优估计理论，采用状态空间描述法，能处理多维和非平稳的随机过程。
状态方程：
x
k
=
A
x
k
−
1
+
B
u
k
+
ω
k
{x_{k}}=A x_{k-1}+B u_{k}+\omega_{k}
xk=Axk−1+Buk+ωk
- xk：表示当前状态的当前值 K代表当前的状态信息
- xk-1：表示上一个状态的当前值 K-1代表上一个状态信息
- wk：表示过程噪声
- uk:表示的是一个输入信息
- A： 表示的是一个状态转移矩阵
- B： 表示的是一个控制矩阵
观测方程：
y
k
=
C
x
k
+
v
k
y_{k}=C x_{k}+v_{k}
yk=Cxk+vk
- yk:表示要观测的值
- vk:表示一个过程噪声（与观察器误差有关可以简单理解为误差）
- xk：表示当前状态的当前值 K代表当前的状态信息
卡尔曼滤波的理解：
实现过程: 使用上一次的最优结果预测当前的值,同时使用观测值****修正当前值,得到最优结果(可以简单理解为是一个递归的过程)
在预测过程中使用到的公式。
x ^ t − = F x ^ t − 1 + B u t − 1 \hat{x}_{t}^{-}=F \hat{x}_{t-1}+B u_{t-1}x^t−=Fx^t−1+But−1
P t − = F P t − 1 F T + Q P_{t}^{-}=F P_{t-1} F^{T}+QPt−=FPt−1FT+Q
在更新过程中使用到的公式
K k = P k − H T H P k − H T + R K_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R}Kk=HPk−HT+RPk−HT
x ^ t = x ^ t − + K t ( z t − H x ^ t − ) \hat{x}_{t}=\hat{x}_{t}^{-}+K_{t}\left(z_{t}-H \hat{x}_{t}^{-}\right)x^t=x^t−+Kt(zt−Hx^t−)
P t = ( I − K t H ) P t − P_{t}=\left(I-K_{t} H\right) P_{t}^{-}Pt=(I−KtH)Pt−
跟踪场景的应用
- 状态预测：新的最优估计是根据上一最优估计预测得到
基于track在t - 1时刻的状态来预测其在t时刻的状态
x
′
=
F
x
x^{\prime}=F x
x′=Fx
x为track在t－1时刻的均值，F称为状态转移矩阵，该公式预测t时刻的x’
(
c
x
c
y
w
h
v
x
v
y
w
w
v
h
)
t
=
(
1
0
0
0
d
t
0
0
0
0
1
0
0
0
d
t
0
0
0
0
1
0
0
0
d
t
0
0
0
0
1
0
0
0
d
t
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
)
⋅
(
c
x
c
y
w
h
v
x
v
y
v
w
v
h
)
\left(\begin{array}{c} c x \\ c y \\ w \\ h \\ v x \\ v y \\ w w \\ v h \end{array}\right)_{t}=\left(\begin{array}{cccccccc} 1 & 0 & 0 & 0 & d t & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & d t & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & d t & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & d t \\ 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} c x \\ c y \\ w \\ h \\ v x \\ v y \\ v w \\ v h \end{array}\right)
cxcywhvxvywwvht=10000000010000000010000000010000dt00010000dt00010000dt00010000dt0001⋅cxcywhvxvyvwvh
- 协方差的预测
新的不确定性由上一不确定性预测得到，并加上外部环境的干扰
P
′
=
F
P
F
T
+
Q
P^{\prime}=F P F^{T}+Q
P′=FPFT+Q
Cov
(
x
)
=
Σ
Cov
(
A
x
)
=
A
Σ
A
T
\begin{aligned} \operatorname{Cov}(x) & =\Sigma \\ \operatorname{Cov}(\mathbf{A} x) & =\mathbf{A} \Sigma \mathbf{A}^{T} \end{aligned}
Cov(x)Cov(Ax)=Σ=AΣAT
状态的更新
在跟踪的场景中对卡尔曼滤波更新公式进行进一步的细化
y
=
z
−
H
x
′
y=z-H x^{\prime}
y=z−Hx′
z为detection的均值向量，不包含速度变化值，即z = [cx, cy,r,h]， H称为测量矩阵， 它将track的均值向量x’映射到测量空间该公式计算detection和track的均值误差，y称为innovation（新息）
S
=
H
P
′
H
T
+
R
K
=
P
′
H
T
S
−
1
x
=
x
′
+
K
y
P
=
(
I
−
K
H
)
P
′
\begin{array}{l} S=H P^{\prime} H^{T}+R \\ K=P^{\prime} H^{T} S^{-1} \\ x=x^{\prime}+K y \\ P=(I-K H) P^{\prime} \end{array}
S=HP′HT+RK=P′HTS−1x=x′+KyP=(I−KH)P′
卡尔曼滤波公式推导
x
^
t
−
=
F
x
^
t
−
1
+
B
u
t
−
1
\hat{x}_{t}^{-}=F \hat{x}_{t-1}+B u_{t-1} \\
x^t−=Fx^t−1+But−1
其中的F即为状态转移矩阵 B即为控制矩阵
P
t
−
=
F
P
t
−
1
F
T
+
Q
P_{t}^{-}=F P_{t-1} F^{T}+Q
Pt−=FPt−1FT+Q
根据先验估计得到协方差的推导过程如下：
cov
(
x
^
t
−
,
x
^
t
−
)
=
cov
(
F
x
^
t
−
1
+
B
u
t
−
1
,
F
x
^
t
−
1
+
B
u
t
−
1
)
+
Q
=
F
cov
(
x
^
t
−
1
,
x
^
t
−
1
)
F
⊤
+
Q
\begin{array}{l} \operatorname{cov}\left(\hat{x}_{t}^{-}, \hat{x}_{t^{-}}\right)=\operatorname{cov}\left(F \hat{x}_{t-1}+B u_{t-1}, F \hat{x}_{t-1}+B u_{t-1}\right) \\ +Q=F \operatorname{cov}\left(\hat{x}_{t-1}, \hat{x}_{t-1}\right) F^{\top} +Q \\ \end{array}
cov(x^t−,x^t−)=cov(Fx^t−1+But−1,Fx^t−1+But−1)+Q=Fcov(x^t−1,x^t−1)F⊤+Q
从而得到了先验估计协方差的形式（带过程噪声的形式）
如果不考虑噪声w或者v，我们可以得到系统k时刻的估计值值为：
x
^
k
ˉ
=
A
x
^
k
−
1
+
B
u
k
−
1
or
x
^
k
m
=
C
−
1
y
k
\hat{x}_{\bar{k}}=A \hat{x}_{k-1}+B u_{k-1} \quad \text { or } \quad \hat{x}_{k_{m}}=C^{-1} y_{k}
x^kˉ=Ax^k−1+Buk−1 or x^km=C−1yk
数据融合:根据滤波思想中的线性加权组合数据。
x
^
k
=
x
^
k
ˉ
+
G
(
x
^
k
m
−
x
^
k
ˉ
)
\hat{x}_{k}=\hat{x}_{\bar{k}}+G\left(\hat{x}_{k_{m}}-\hat{x}_{\bar{k}}\right)
x^k=x^kˉ+G(x^km−x^kˉ)
x
^
k
=
x
^
k
ˉ
+
K
k
(
y
k
−
C
x
^
k
ˉ
)
\hat{x}_{k}=\hat{x}_{\bar{k}}+K_{k}\left(y_{k}-C \hat{x}_{\bar{k}}\right)
x^k=x^kˉ+Kk(yk−Cx^kˉ)
因此我们的目标就变得十分清晰了，就是求使得考虑了噪声之后的估计值趋近于真实值,那么此时引入误差：
e
k
=
X
k
−
X
^
k
e_{k}=X_{k}-\hat{X}_{k}
ek=Xk−X^k
同理P
(
e
k
)
∼
(
0
,
P
)
,
P
也是那个协方差矩阵
\text { 同理P }\left(e_{k}\right) \sim(0, P), P \text { 也是那个协方差矩阵 }
同理P (ek)∼(0,P),P 也是那个协方差矩阵
而当在不同的维度上的方差越小，那么说明这个e越接近0，因此估计值和真实值也就是最相近的。所以，要选择合适的K使得tr（P）（矩阵对角线相加)最小，那么优化问题就变成了下面这个公式。
P
=
E
(
e
k
e
k
T
)
=
E
(
(
X
k
−
X
^
k
)
(
X
k
−
X
^
k
)
T
)
,
将
X
^
k
=
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
代入,
\begin{array}{l} \mathrm{P}=\mathrm{E}\left(e_{k} e_{k}{ }^{T}\right)=\mathrm{E}\left(\left(X_{k}-\hat{X}_{k}\right)\left(X_{k}-\hat{X}_{k}\right)^{T}\right), \\ \text { 将 } \hat{X}_{k}=\hat{X}_{k}^{-}+K_{k}\left(Z_{k}-H \hat{X}_{k}^{-}\right) \text {代入, } \\ \end{array}
P=E(ekekT)=E((Xk−X^k)(Xk−X^k)T), 将 X^k=X^k−+Kk(Zk−HX^k−)代入,
P
k
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
=
E
[
(
x
k
−
x
^
k
ˉ
−
K
k
(
z
k
−
C
x
^
k
ˉ
)
)
(
x
k
−
x
^
k
ˉ
−
K
k
(
z
k
−
C
x
^
k
ˉ
)
)
T
]
=
E
[
(
x
k
−
x
^
k
ˉ
−
K
k
(
C
x
k
+
v
k
−
C
x
^
k
ˉ
)
)
(
x
k
−
x
^
k
ˉ
−
K
k
(
C
x
k
+
v
k
−
C
x
^
k
ˉ
)
)
T
]
\begin{aligned} P_{k} & =E\left[\left(x_{k}-\hat{x}_{k}\right)\left(x_{k}-\hat{x}_{k}\right)^{T}\right] \\ & =E\left[\left(x_{k}-\hat{x}_{\bar{k}}-K_{k}\left(z_{k}-C \hat{x}_{\bar{k}}\right)\right)\left(x_{k}-\hat{x}_{\bar{k}}-K_{k}\left(z_{k}-C \hat{x}_{\bar{k}}\right)\right)^{T}\right] \\ & =E\left[\left(x_{k}-\hat{x}_{\bar{k}}-K_{k}\left(C x_{k}+v_{k}-C \hat{x}_{\bar{k}}\right)\right)\left(x_{k}-\hat{x}_{\bar{k}}-K_{k}\left(C x_{k}+v_{k}-C \hat{x}_{\bar{k}}\right)\right)^{T}\right] \end{aligned}
Pk=E[(xk−x^k)(xk−x^k)T]=E[(xk−x^kˉ−Kk(zk−Cx^kˉ))(xk−x^kˉ−Kk(zk−Cx^kˉ))T]=E[(xk−x^kˉ−Kk(Cxk+vk−Cx^kˉ))(xk−x^kˉ−Kk(Cxk+vk−Cx^kˉ))T]
定义先验估计误差
e
k
ˉ
=
x
k
−
x
^
k
ˉ
且用
P
k
ˉ
来表示先验估计误差的协方差, 则有
\text { 定义先验估计误差 } e_{\bar{k}}=x_{k}-\hat{x}_{\bar{k}} \text { 且用 } P_{\bar{k}} \text { 来表示先验估计误差的协方差, 则有 }
定义先验估计误差 ekˉ=xk−x^kˉ 且用 Pkˉ 来表示先验估计误差的协方差, 则有
P
k
=
E
[
(
e
k
ˉ
−
K
k
(
v
k
+
C
e
k
ˉ
)
(
e
k
ˉ
−
K
k
(
v
k
−
C
e
k
ˉ
)
T
]
=
E
[
(
(
I
−
K
k
C
)
e
k
ˉ
−
K
k
v
k
)
(
(
I
−
K
k
C
)
e
k
ˉ
−
K
k
v
k
)
T
]
=
E
[
(
I
−
K
k
C
)
e
k
ˉ
e
k
ˉ
T
(
I
−
K
k
C
)
T
+
K
k
v
k
v
k
T
K
k
T
]
+
E
[
(
I
−
K
k
C
)
e
k
ˉ
v
k
T
K
k
T
]
+
E
[
K
k
v
k
e
k
ˉ
T
(
I
−
K
k
C
)
]
=
E
[
(
I
−
K
k
C
)
e
k
ˉ
e
k
ˉ
T
(
I
−
K
k
C
)
T
+
K
k
v
k
v
k
T
K
k
T
]
=
(
I
−
K
k
C
)
P
k
ˉ
(
I
−
K
k
C
)
T
+
K
k
R
K
k
T
\begin{aligned} P_{k} & =E\left[\left(e_{\bar{k}}-K_{k}\left(v_{k}+C e_{\bar{k}}\right)\left(e_{\bar{k}}-K_{k}\left(v_{k}-C e_{\bar{k}}\right)^{T}\right]\right.\right. \\ & =E\left[\left(\left(I-K_{k} C\right) e_{\bar{k}}-K_{k} v_{k}\right)\left(\left(I-K_{k} C\right) e_{\bar{k}}-K_{k} v_{k}\right)^{T}\right] \\ & =E\left[\left(I-K_{k} C\right) e_{\bar{k}} e_{\bar{k}}^{T}\left(I-K_{k} C\right)^{T}+K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right]+E\left[\left(I-K_{k} C\right) e_{\bar{k}} v_{k}^{T} K_{k}^{T}\right]+E\left[K_{k} v_{k} e_{\bar{k}}^{T}\left(I-K_{k} C\right)\right] \\ & =E\left[\left(I-K_{k} C\right) e_{\bar{k}} e_{\bar{k}}^{T}\left(I-K_{k} C\right)^{T}+K_{k} v_{k} v_{k}^{T} K_{k}^{T}\right] \\ & =\left(I-K_{k} C\right) P_{\bar{k}}\left(I-K_{k} C\right)^{T}+K_{k} R K_{k}^{T} \end{aligned}
Pk=E[(ekˉ−Kk(vk+Cekˉ)(ekˉ−Kk(vk−Cekˉ)T]=E[((I−KkC)ekˉ−Kkvk)((I−KkC)ekˉ−Kkvk)T]=E[(I−KkC)ekˉekˉT(I−KkC)T+KkvkvkTKkT]+E[(I−KkC)ekˉvkTKkT]+E[KkvkekˉT(I−KkC)]=E[(I−KkC)ekˉekˉT(I−KkC)T+KkvkvkTKkT]=(I−KkC)Pkˉ(I−KkC)T+KkRKkT
tr
[
P
k
]
=
tr
[
(
I
−
K
k
C
)
P
k
ˉ
(
I
−
K
k
C
)
T
+
K
k
R
K
k
T
]
=
tr
[
(
P
k
ˉ
−
K
k
C
P
k
ˉ
)
(
I
−
C
T
K
k
T
)
+
K
k
R
K
k
T
]
=
tr
[
P
k
ˉ
−
K
k
C
P
k
ˉ
−
P
k
ˉ
C
T
K
k
T
+
K
k
C
P
k
ˉ
C
T
K
k
T
+
K
k
R
K
k
T
]
=
tr
[
P
k
ˉ
−
2
K
k
C
P
k
ˉ
+
K
k
C
P
k
ˉ
C
T
K
k
T
+
K
k
R
K
k
T
]
\begin{aligned} \operatorname{tr}\left[P_{k}\right] & =\operatorname{tr}\left[\left(I-K_{k} C\right) P_{\bar{k}}\left(I-K_{k} C\right)^{T}+K_{k} R K_{k}^{T}\right] \\ & =\operatorname{tr}\left[\left(P_{\bar{k}}-K_{k} C P_{\bar{k}}\right)\left(I-C^{T} K_{k}^{T}\right)+K_{k} R K_{k}^{T}\right] \\ & =\operatorname{tr}\left[P_{\bar{k}}-K_{k} C P_{\bar{k}}-P_{\bar{k}} C^{T} K_{k}^{T}+K_{k} C P_{\bar{k}} C^{T} K_{k}^{T}+K_{k} R K_{k}^{T}\right] \\ & =\operatorname{tr}\left[P_{\bar{k}}-2 K_{k} C P_{\bar{k}}+K_{k} C P_{\bar{k}} C^{T} K_{k}^{T}+K_{k} R K_{k}^{T}\right] \end{aligned}
tr[Pk]=tr[(I−KkC)Pkˉ(I−KkC)T+KkRKkT]=tr[(Pkˉ−KkCPkˉ)(I−CTKkT)+KkRKkT]=tr[Pkˉ−KkCPkˉ−PkˉCTKkT+KkCPkˉCTKkT+KkRKkT]=tr[Pkˉ−2KkCPkˉ+KkCPkˉCTKkT+KkRKkT]
则下一步就是对卡尔曼增益K求导，求出极值点，则认为该点处的估计误差的协方差的迹tr（P)最小，求导并使之为0如下：
d
(
tr
(
P
k
)
)
d
(
K
k
)
=
−
2
P
k
ˉ
T
C
+
2
K
k
C
P
k
ˉ
C
T
+
2
K
k
R
=
0
d
(
tr
(
A
B
)
d
A
=
B
T
d
(
tr
(
A
B
A
T
)
)
d
A
=
A
(
B
+
B
T
)
\begin{aligned} \frac{d\left(\operatorname{tr}\left(P_{k}\right)\right)}{d\left(K_{k}\right)}= & -2 P_{\bar{k}}^{T} C+2 K_{k} C P_{\bar{k}} C^{T}+2 K_{k} R=0 \\ & \frac{d(\operatorname{tr}(A B)}{d A}=B^{T} \\ & \frac{d\left(\operatorname{tr}\left(A B A^{T}\right)\right)}{d A}=A\left(B+B^{T}\right) \end{aligned}
d(Kk)d(tr(Pk))=−2PkˉTC+2KkCPkˉCT+2KkR=0dAd(tr(AB)=BTdAd(tr(ABAT))=A(B+BT)
最后我们就可以得到卡尔曼增益的推导公式：
P
k
ˉ
T
C
+
K
k
(
C
P
k
ˉ
C
T
+
R
)
=
0
,
即
,
K
k
=
P
k
ˉ
T
C
C
P
k
ˉ
C
T
+
R
P_{\bar{k}}^{T} C+K_{k}\left(C P_{\bar{k}} C^{T}+R\right)=0 , 即, K_{k}=\frac{P_{\bar{k}}^{T} C}{C P_{\bar{k}} C^{T}+R}
PkˉTC+Kk(CPkˉCT+R)=0,即,Kk=CPkˉCT+RPkˉTC
之后对于误差协方差矩阵的更新公式的推导过程就比较简单了。只需要将卡尔曼滤波的值带入PK中进行求解就可以得到。
P
k
=
(
I
−
K
k
C
)
P
k
ˉ
P_{k}=\left(I-K_{k} C\right) P_{\bar{k}}
Pk=(I−KkC)Pkˉ
本文转载自: https://blog.csdn.net/weixin_46167190/article/details/141087283
版权归原作者 程序小旭 所有， 如有侵权，请联系我们删除。
版权归原作者 程序小旭 所有， 如有侵权，请联系我们删除。