SORT目标追踪
前言
SORT(Simple Online and Realtime Tracking)算法是一种简单的在线实时多目标跟踪算法，主要利用卡尔曼滤波来传播目标物体到未来帧中，再通过IOU作为度量指标来建立关系
提示：以下是本篇文章正文内容，下面案例可供参考
一、卡尔曼滤波器
Kalman Filter的Filter并不能很好的体现出它的特性。
卡尔曼滤波器一言以蔽之，就是Optimal Recursive Data Processing Algorithm，最优化递归数字处理算法。
它更像一种观测器，而不是一种滤波器。
1.迭代算法
真实的世界中充满了不确定性，当描述一个系统时，不确定性主要来自三个方面：
① 不存在完美的数学模型
② 系统的扰动不可控，也很难建模 （系统噪声）
③测量传感器存在误差 （测量噪声）
假设有一把尺子，取测量一个硬币的直径，第一次测得50.1mm，第二次测得50.4mm，第三次测得49.8mm，求真实数据值，最直观的就是求平均值
估计一个真实数据，大致的方法就是取平均值
x
k
^
\hat{x_k}
xk^：第k次的估计值。Hat表示先验估计
Z
k
Z_k
Zk：第K次的测量值
x
^
k
=
1
k
(
Z
1
+
Z
2
+
Z
3
+
⋯
+
Z
k
)
=
1
k
k
−
1
k
−
1
(
Z
1
+
Z
2
+
Z
3
+
⋯
+
Z
k
−
1
)
+
1
k
Z
k
=
k
−
1
k
x
^
k
−
1
+
1
k
Z
k
=
x
^
k
−
1
+
1
k
(
Z
k
−
x
^
k
−
1
)
\hat{x}_k=\frac{1}{k}(Z_1+Z_2+Z_3+\cdots+Z_k)\\ =\frac{1}{k}\frac{k-1}{k-1}(Z_1+Z_2+Z_3+\cdots+Z_k-1)+\frac{1}{k}Z_k\\ =\frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}Z_k\\ =\hat{x}_{k-1}+\frac{1}{k}(Z_k-\hat{x}_{k-1})
x^k=k1(Z1+Z2+Z3+⋯+Zk)=k1k−1k−1(Z1+Z2+Z3+⋯+Zk−1)+k1Zk=kk−1x^k−1+k1Zk=x^k−1+k1(Zk−x^k−1)
分析这个公式，随着K增大， 1/K减小，Xk趋向于Xk-1。也就是随着K增加，测量结果不再重要了。
当k小时，1/k 大，Zk的作用较大，也就是测量值的作用比较大。
也就是当前的估计值 = 上一时刻的估计值 + 系数 * （当前时刻测量值 - 上一次的估计值）
此时的系数即为Kalman Gain，卡尔曼增益/因数
这样迭代求估计值的过程为：
①计算Kalman Gain
K
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
K_k = \frac{ e_{EST_{k-1}} } { e_{EST_{k-1}} + e_{MEA_{k}} }
Kk=eESTk−1+eMEAkeESTk−1
②计算先验估计值
x
k
^
=
x
^
k
−
1
+
K
k
(
Z
k
−
x
^
k
−
1
)
\hat{x_{k}}=\hat{x}_{k-1}+K_k(Z_k-\hat{x}_{k-1})
xk^=x^k−1+Kk(Zk−x^k−1)
③更新估计误差
e
E
S
T
k
=
(
1
−
K
k
)
e
E
S
T
k
−
1
e_{EST_{k}}=(1-K_k)e_{EST_{k-1}}
eESTk=(1−Kk)eESTk−1
用一把精度为（3mm, 0.25）的尺子去测量一个5cm卡片的长度，测得的长度分别为47mm,52mm,53mm,51mm,48mm,49mm,49mm,48mm,49mm,51mm,50mm，求卡片的真实长度
①起始估计值设为40mm，估计误差为8mm
②依次计算Kalman增益、先验估计值、更新估计误差
也可以从上面的计算过程可以看出，卡尔曼滤波其实就是选择了最佳加权系数的加权平均
2.数据融合
有两把把尺子
z
1
=
30
m
m
,
δ
=
2
m
m
z_1=30mm,\delta=2mm
z1=30mm,δ=2mm
z
2
=
32
m
m
,
δ
=
4
m
m
z_2=32mm,\delta=4mm
z2=32mm,δ=4mm
通过这两把尺子的测量值，估算真实值
上文介绍的迭代算法，可以得到
z
^
=
z
1
+
k
(
z
2
−
z
1
)
\hat{z}=z_1+k(z_2-z_1)
z^=z1+k(z2−z1)
根据最小方差无偏估计，我们要求出一个k使得方差最小
δ
z
^
2
=
v
a
r
(
z
1
+
k
(
z
2
−
z
1
)
)
=
v
a
r
(
(
1
−
k
)
z
1
+
k
z
2
)
=
v
a
r
(
(
1
−
k
)
(
z
1
)
)
+
v
a
r
(
k
z
2
)
=
(
1
−
k
)
2
δ
z
1
2
+
k
2
δ
z
2
2
\delta_{\hat{z}}^2=var(z_1+k(z_2-z_1)) \\=var((1-k)z_1+kz_2) \\=var((1-k)(z_1))+var(kz_2) \\=(1-k)^2\delta_{z_1}^2+k^2\delta_{z_2}^2
δz^2=var(z1+k(z2−z1))=var((1−k)z1+kz2)=var((1−k)(z1))+var(kz2)=(1−k)2δz12+k2δz22
令
δ
2
k
=
0
\frac{\delta^2}{k}=0
kδ2=0，解得：
k
=
δ
1
2
δ
1
2
+
δ
1
2
=
2
∗
2
2
∗
2
+
4
∗
4
=
0.2
k=\frac{{\delta_1}^2}{ {\delta_1}^2+ {\delta_1}^2} = \frac{2*2}{2*2+4*4}=0.2
k=δ12+δ12δ12=2∗2+4∗42∗2=0.2
z
^
=
30
+
0.2
∗
(
32
−
30
)
=
30.4
\hat{z}=30+0.2*(32-30)=30.4
z^=30+0.2∗(32−30)=30.4
δ
z
^
2
=
(
1
−
0.2
)
∗
(
1
−
0.2
)
∗
2
∗
2
+
0.2
∗
0.2
∗
4
∗
4
=
3.2
\delta_{\hat{z}}^2=(1-0.2)*(1-0.2)*2*2+0.2*0.2*4*4=3.2
δz^2=(1−0.2)∗(1−0.2)∗2∗2+0.2∗0.2∗4∗4=3.2
3.协方差矩阵
协方差矩阵就是将方差与协方差在一个矩阵中表现出来，表示变量间的联动关系
协方差
δ
x
δ
y
\delta_x\delta_y
δxδy表示两个变量的变化方向，大于0表示正相关
一般协方差矩阵表示为
[
δ
x
2
δ
x
δ
y
δ
x
δ
z
δ
y
δ
x
δ
y
2
δ
y
δ
z
δ
z
δ
x
δ
z
δ
y
δ
z
2
]
\begin{bmatrix} {\delta_x}^2 & \delta_x\delta_y & \delta_x\delta_z \\ \delta_y\delta_x & {\delta_y}^2 & \delta_y\delta_z \\ \delta_z\delta_x & \delta_z\delta_y & {\delta_z}^2 \end{bmatrix}
⎣⎡δx2δyδxδzδxδxδyδy2δzδyδxδzδyδzδz2⎦⎤
接下来介绍一个求解协方差矩阵的方法，方便编程计算协方差矩阵
引入过渡矩阵a
a
=
[
x
1
y
1
z
1
x
2
y
2
z
2
x
3
y
3
z
3
]
−
1
3
[
1
1
1
1
1
1
1
1
1
]
[
x
1
y
1
z
1
x
2
y
2
z
2
x
3
y
3
z
3
]
a = \begin{bmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{bmatrix}-\frac{1}{3}\begin{bmatrix} 1& 1 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{bmatrix}
a=⎣⎡x1x2x3y1y2y3z1z2z3⎦⎤−31⎣⎡111111111⎦⎤⎣⎡x1x2x3y1y2y3z1z2z3⎦⎤
P
=
1
3
a
T
a
P = \frac{1}{3}a^Ta
P=31aTa
4.状态空间表达
在这里引入弹簧阻尼系统来介绍状态空间表达式
m
x
¨
+
B
x
˙
+
k
x
=
F
m\ddot{x}+B\dot{x}+kx=F
mx¨+Bx˙+kx=F
引入状态变量：
x
1
=
x
x_1=x
x1=x
x
2
=
x
˙
x_2=\dot{x}
x2=x˙
把F看作u作为系统的一个输入：
x
1
˙
=
x
2
\dot{x_1}=x_2
x1˙=x2
x
2
˙
=
x
¨
=
1
m
u
−
1
m
B
x
˙
−
1
m
k
x
=
x
¨
=
1
m
u
−
1
m
B
x
2
−
1
m
k
x
1
\dot{x_2}=\ddot{x}=\frac{1}{m}u-\frac{1}{m}B\dot{x}-\frac{1}{m}kx=\ddot{x}=\frac{1}{m}u-\frac{1}{m}Bx_2-\frac{1}{m}kx_1
x2˙=x¨=m1u−m1Bx˙−m1kx=x¨=m1u−m1Bx2−m1kx1
[
x
1
˙
x
2
˙
]
=
[
0
1
−
m
k
−
m
B
]
[
x
1
x
2
]
+
[
0
1
m
]
u
\begin{bmatrix} \dot{x_1} \\ \dot{x_2} \end{bmatrix}=\begin{bmatrix} 0 & 1 \\ -\frac{m}{k} & -\frac{m}{B} \end{bmatrix} \begin{bmatrix} x_1 \\ x_2\end{bmatrix}+\begin{bmatrix} 0 \\ \frac{1}{m} \end{bmatrix}u
[x1˙x2˙]=[0−km1−Bm][x1x2]+[0m1]u
[
z
1
z
2
]
=
[
1
0
0
1
]
[
x
1
x
2
]
\begin{bmatrix} z_1 \\ z_2 \end{bmatrix} = \begin{bmatrix} 1&0 \\ 0&1 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2\end{bmatrix}
[z1z2]=[1001][x1x2]
离散化之后，可以总结为：
X
k
=
A
X
k
−
1
+
B
u
+
w
k
−
1
X_k=AX_{k-1}+Bu+w_{k-1}
Xk=AXk−1+Bu+wk−1 过程噪音
Z
k
=
H
X
k
+
v
k
Z_k=HX_k+v_k
Zk=HXk+vk 测量噪声
那么便引出了核心问题：如何在一个不那么准确的估计值和一个不那么准确的测量值之间，估算出一个较为准确的 X k ^ \hat{X_k} Xk^，这也就是卡尔曼滤波要解决的事情
5.卡尔曼滤波详细推导
状态空间方程：
X
k
=
A
X
k
−
1
+
B
U
k
−
1
+
W
k
−
1
Z
k
=
H
X
k
+
V
k
X_k=AX_{k-1}+BU_{k-1}+W_{k-1} \\ Z_k=HX_k+V_k
Xk=AXk−1+BUk−1+Wk−1Zk=HXk+Vk
但是
w
k
−
1
w_{k-1}
wk−1和
v
k
v_k
vk是无法建模的，不可知的。因此只能列出下面两个公式
X
k
−
^
=
A
X
k
−
1
^
+
B
U
k
−
1
①
Z
k
=
H
X
k
+
V
k
−
−
−
>
x
^
k
M
E
A
=
H
−
1
Z
k
②
\hat{X_k^-}=\hat{AX_{k-1}}+BU_{k-1} ① \\ Z_k=HX_k+V_k ---> \hat{x}_{k_{MEA}}=H^{-1} Z_k ②
Xk−^=AXk−1^+BUk−1①Zk=HXk+Vk−−−>x^kMEA=H−1Zk②
x
^
\hat{x}
x^表示
x
x
x的估计值，
x
^
−
\hat{x}^{-}
x^−表示
x
x
x的先验估计值，只通过状态空间方程得到，没有经过任何处理
这样，我们得到一个算出来的结果
X
k
−
^
\hat{X_k^-}
Xk−^和一个测出来的结果
Z
k
Z_k
Zk，这两个数值都是不准确的，那么怎么得到一个较为精确的数值呢。自然想到我们上面介绍的数据融合。
X
k
^
=
X
k
−
^
+
G
(
H
−
1
Z
k
−
X
k
−
^
)
\hat{X_k}=\hat{X_k^{-}}+G(H^{-1}Z_k-\hat{X_k^{-}})
Xk^=Xk−^+G(H−1Zk−Xk−^)
教科书中一般写成如下公式：
G
=
K
k
H
G=K_kH
G=KkH
X
k
^
=
X
k
−
^
+
K
k
(
Z
k
−
H
X
k
−
^
)
\hat{X_k}=\hat{X_k^{-}}+K_k(Z_k-H\hat{X_k^{-}})
Xk^=Xk−^+Kk(Zk−HXk−^)
这样写应该是为了防止
H
H
H矩阵不可逆吧。
那么我们的目标就是寻找
K
k
K_k
Kk，使得
X
k
^
\hat{X_k}
Xk^更加接近于
X
k
X_k
Xk(真实值)
e
k
=
X
k
−
X
k
^
e_k = X_k-\hat{X_k}
ek=Xk−Xk^
w
k
−
1
w_{k-1}
wk−1是过程噪声，我们假设真实世界的噪声是符合正态分布的，
e
k
e_k
ek满足正态分布，
e
k
e_k
ek的协方差矩阵：
P
=
[
e
e
T
]
=
[
σ
e
1
2
σ
e
1
e
2
σ
e
1
e
2
σ
e
2
2
]
P = \begin{bmatrix} e & e^T \end{bmatrix}=\begin{bmatrix} \sigma e_1^2 &\sigma e_1e_2\\ \sigma e_1e_2 &\sigma e_2^2 \end{bmatrix}
P=[eeT]=[σe12σe1e2σe1e2σe22]
e
k
e_k
ek的期望最小，等价于方差最小，等价于P的迹最小
(
A
B
)
T
=
B
T
A
T
(
A
+
B
)
T
=
A
T
+
B
T
(AB)^T=B^TA^T\\(A+B)^T=A^T+B^T
(AB)T=BTAT(A+B)T=AT+BT
x
k
−
x
k
^
=
x
k
−
x
k
−
^
−
K
k
(
Z
k
−
H
x
k
−
^
)
=
x
k
−
x
k
−
^
−
K
k
(
H
X
k
+
V
k
−
H
x
k
−
^
)
=
(
x
k
−
x
k
−
^
)
−
K
k
H
(
x
k
−
x
k
−
^
)
−
K
k
V
k
=
(
I
−
K
k
H
)
(
x
k
−
x
k
−
^
)
−
K
k
V
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
V
k
x_k-\hat{x_k}=x_k-\hat{x_k^-}-K_k(Z_k-H\hat{x_k^-}) \\= x_k-\hat{x_k^-}-K_k(HX_k+V_k-H\hat{x_k^-})\\=(x_k-\hat{x_k^-})-K_kH(x_k-\hat{x_k^-})-K_kV_k\\=(I-K_kH)(x_k-\hat{x_k^-})-K_kV_k\\=(I-K_kH)e_k^--K_kV_k
xk−xk^=xk−xk−^−Kk(Zk−Hxk−^)=xk−xk−^−Kk(HXk+Vk−Hxk−^)=(xk−xk−^)−KkH(xk−xk−^)−KkVk=(I−KkH)(xk−xk−^)−KkVk=(I−KkH)ek−−KkVk
P k = E [ e e T ] = E [ ( x k − x k ^ ) ( x k − x k ^ ) T ] = E [ ( ( I − K k H ) e k − − K k V k ( ( I − K k H ) e k − − K k V k ) T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T ] − E [ ( I − K k H ) e k − V k T K k T ] − E [ K k V k e k − T ( I − K k H ) T ] + E [ K k V k V k T K k T ] = ( I − K k H ) E ( e k − e k − T ( I − K k H ) T ) + K k E ( V k V k T ) K k T = ( P k − − K k H P k − ) ( I T − H T K k T ) + K k R K k T = P k − − K k H P K − − P k − H T K k T + K k H P k − H T K k T = K k P K k T P_k=E\begin{bmatrix} e & eT \end{bmatrix}=E\begin{bmatrix} (x_k-\hat{x_k}) & (x_k-\hat{x_k})^T \end{bmatrix}\\=E\begin{bmatrix} ((I-K_kH)e_k^--K_kV_k &((I-K_kH)e_k^--K_kV_k)^T \end{bmatrix} \\=E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T]-E[(I-K_kH)e_k^-V_k^TK_k^T]-E[K_kV_ke_k^{-T}(I-K_kH)^T]+E[K_kV_kV_k^TK_k^T]\\=(I-K_kH)E(e_k^-e_k^{-T}(I-K_kH)^T)+K_kE(V_kV_k^T)K_k^T\\=(P_k^--K_kHP_k^-)(I^T-H^TK_k^T)+K_kRK_k^T\\=P_k^- -K_kHP_K^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T=K_kPK_k^T Pk=E[eeT]=E[(xk−xk^)(xk−xk^)T]=E[((I−KkH)ek−−KkVk((I−KkH)ek−−KkVk)T]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−VkTKkT]−E[KkVkek−T(I−KkH)T]+E[KkVkVkTKkT]=(I−KkH)E(ek−ek−T(I−KkH)T)+KkE(VkVkT)KkT=(Pk−−KkHPk−)(IT−HTKkT)+KkRKkT=Pk−−KkHPK−−Pk−HTKkT+KkHPk−HTKkT=KkPKkT
K k H P k − t r ( P k ) = t r ( P k − ) − 2 t r ( K k H P k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) K_kHP_k^- tr(P_k) =tr(P_k^-)-2tr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T) KkHPk−tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
d
t
r
(
P
k
)
d
K
k
=
0
−
2
(
H
P
K
−
)
T
+
2
K
k
H
P
k
−
H
T
+
2
K
k
R
\frac{dtr(P_k)}{dK_k}=0-2(HP_K^-)^T+2K_kHP_k^-H^T+2K_kR
dKkdtr(Pk)=0−2(HPK−)T+2KkHPk−HT+2KkR
K
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}
Kk=HPk−HT+RPk−HT
7541
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
