SORT目标追踪算法详解_sort算法

SORT目标追踪

---

前言

SORT(Simple Online and Realtime Tracking)算法是一种简单的在线实时多目标跟踪算法，主要利用卡尔曼滤波来传播目标物体到未来帧中，再通过IOU作为度量指标来建立关系

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、卡尔曼滤波器

Kalman Filter的Filter并不能很好的体现出它的特性。
卡尔曼滤波器一言以蔽之，就是Optimal Recursive Data Processing Algorithm，最优化递归数字处理算法。
它更像一种观测器，而不是一种滤波器。

1.迭代算法

真实的世界中充满了不确定性，当描述一个系统时，不确定性主要来自三个方面：
① 不存在完美的数学模型
② 系统的扰动不可控，也很难建模 （系统噪声）
③测量传感器存在误差 （测量噪声）

---

假设有一把尺子，取测量一个硬币的直径，第一次测得50.1mm，第二次测得50.4mm，第三次测得49.8mm，求真实数据值，最直观的就是求平均值

估计一个真实数据，大致的方法就是取平均值

$\widehat{x_k}$：第k次的估计值。Hat表示先验估计
$Z_k$：第K次的测量值
$$\begin{gathered} {\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}) \end{gathered}$$
分析这个公式，随着K增大， 1/K减小，Xk趋向于Xk-1。也就是随着K增加，测量结果不再重要了。
当k小时，1/k 大，Zk的作用较大，也就是测量值的作用比较大。

也就是当前的估计值 = 上一时刻的估计值 + 系数 * （当前时刻测量值 - 上一次的估计值）
此时的系数即为Kalman Gain，卡尔曼增益/因数
这样迭代求估计值的过程为：
①计算Kalman Gain $K_k=\frac{e_{ES{T}_{k-1}}}{e_{ES{T}_{k-1}}+e_{ME{A}_k}}$
②计算先验估计值 $\widehat{x_k}={\hat{x}}_{k-1}+K_k(Z_k-{\hat{x}}_{k-1})$
③更新估计误差 $e_{ES{T}_k}=(1-K_k)e_{ES{T}_{k-1}}$

用一把精度为（3mm, 0.25）的尺子去测量一个5cm卡片的长度，测得的长度分别为47mm,52mm,53mm,51mm,48mm,49mm,49mm,48mm,49mm,51mm,50mm，求卡片的真实长度
①起始估计值设为40mm，估计误差为8mm
②依次计算Kalman增益、先验估计值、更新估计误差

也可以从上面的计算过程可以看出，卡尔曼滤波其实就是选择了最佳加权系数的加权平均

2.数据融合

有两把把尺子
$z_1=30mm,\delta =2mm$
$z_2=32mm,\delta =4mm$
通过这两把尺子的测量值，估算真实值
上文介绍的迭代算法，可以得到
$\hat{z}=z_1+k(z_2-z_1)$
根据最小方差无偏估计，我们要求出一个k使得方差最小
$$\begin{gathered} {\delta }_{\hat{z}}^2=var(z_1+k(z_2-z_1)) \\ =var((1-k)z_1+k{z}_2) \\ =var((1-k)(z_1))+var(k{z}_2) \\ =(1-k{)}^2{\delta }_{z_1}^2+k^2{\delta }_{z_2}^2 \end{gathered}$$
令 $\frac{{\delta }^2}{k}=0$，解得：
$k=\frac{{{\delta }_1}^2}{{{\delta }_1}^2+{{\delta }_1}^2}=\frac{2\ast 2}{2\ast 2+4\ast 4}=0.2$
$\hat{z}=30+0.2\ast (32-30)=30.4$
${\delta }_{\hat{z}}^2=(1-0.2)\ast (1-0.2)\ast 2\ast 2+0.2\ast 0.2\ast 4\ast 4=3.2$

3.协方差矩阵

协方差矩阵就是将方差与协方差在一个矩阵中表现出来，表示变量间的联动关系
协方差${\delta }_x{\delta }_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}$$ ⎣⎡​δx​2δy​δx​δz​δx​​δx​δy​δy​2δz​δy​​δx​δz​δy​δz​δz​2​⎦⎤​
接下来介绍一个求解协方差矩阵的方法，方便编程计算协方差矩阵
引入过渡矩阵a
$$a=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]-\frac{1}{3}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$$
$$P=\frac{1}{3}{a}^{T}a$$

4.状态空间表达

在这里引入弹簧阻尼系统来介绍状态空间表达式
$$m\ddot{x}+B\dot{x}+kx=F$$
引入状态变量：
$x_1=x$
$x_2=\dot{x}$
把F看作u作为系统的一个输入：
$\dot{x_1}=x_2$
$\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}B{x}_2-\frac{1}{m}k{x}_1$
[ 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−km​​1−Bm​​][x1​x2​​]+[0m1​​]u
$$\left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]$$
离散化之后，可以总结为：
$X_k=A{X}_{k-1}+Bu+w_{k-1}$ 过程噪音
$Z_k=H{X}_k+v_k$ 测量噪声

那么便引出了核心问题：如何在一个不那么准确的估计值和一个不那么准确的测量值之间，估算出一个较为准确的$\widehat{X_k}$，这也就是卡尔曼滤波要解决的事情

5.卡尔曼滤波详细推导

状态空间方程：
$$\begin{gathered} X_k=A{X}_{k-1}+B{U}_{k-1}+W_{k-1} \\ {Z}_k=H{X}_k+V_k \end{gathered}$$
但是$w_{k-1}$和$v_k$是无法建模的，不可知的。因此只能列出下面两个公式
$$\begin{gathered} \widehat{X_k^{-}}=\widehat{A{X}_{k-1}}+B{U}_{k-1}① \\ {Z}_k=H{X}_k+V_k--->{\hat{x}}_{k_{MEA}}=H^{-1}{Z}_k② \end{gathered}$$
$\hat{x}$表示$x$的估计值，${\hat{x}}^{-}$表示$x$的先验估计值，只通过状态空间方程得到，没有经过任何处理
这样，我们得到一个算出来的结果$\widehat{X_k^{-}}$和一个测出来的结果$Z_k$，这两个数值都是不准确的，那么怎么得到一个较为精确的数值呢。自然想到我们上面介绍的数据融合。
$\widehat{X_k}=\widehat{X_k^{-}}+G(H^{-1}{Z}_k-\widehat{X_k^{-}})$
教科书中一般写成如下公式：
$G=K_{k}H$
$\widehat{X_k}=\widehat{X_k^{-}}+K_k(Z_k-H\widehat{X_k^{-}})$
这样写应该是为了防止$H$矩阵不可逆吧。
那么我们的目标就是寻找$K_k$，使得$\widehat{X_k}$更加接近于$X_k$(真实值)
$$e_k=X_k-\widehat{X_k}$$
$w_{k-1}$是过程噪声，我们假设真实世界的噪声是符合正态分布的，$e_k$满足正态分布，$e_k$的协方差矩阵：
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=[e​eT​]=[σe12​σe1​e2​​σe1​e2​σe22​​]
$e_k$的期望最小，等价于方差最小，等价于P的迹最小
$$\begin{gathered} (AB{)}^T=B^T{A}^T \\ (A+B{)}^T=A^T+B^T \end{gathered}$$
$$\begin{gathered} x_k-\widehat{x_k}=x_k-\widehat{x_k^{-}}-K_k(Z_k-H\widehat{x_k^{-}}) \\ =x_k-\widehat{x_k^{-}}-K_k(H{X}_k+V_k-H\widehat{x_k^{-}}) \\ =(x_k-\widehat{x_k^{-}})-K_{k}H(x_k-\widehat{x_k^{-}})-K_k{V}_k \\ =(I-K_{k}H)(x_k-\widehat{x_k^{-}})-K_k{V}_k \\ =(I-K_{k}H)e_k^{-}-K_k{V}_k \end{gathered}$$

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[e​eT​]=E[(xk​−xk​^​)​(xk​−xk​^​)T​]=E[((I−Kk​H)ek−​−Kk​Vk​​((I−Kk​H)ek−​−Kk​Vk​)T​]=E[(I−Kk​H)ek−​ek−T​(I−Kk​H)T]−E[(I−Kk​H)ek−​VkT​KkT​]−E[Kk​Vk​ek−T​(I−Kk​H)T]+E[Kk​Vk​VkT​KkT​]=(I−Kk​H)E(ek−​ek−T​(I−Kk​H)T)+Kk​E(Vk​VkT​)KkT​=(Pk−​−Kk​HPk−​)(IT−HTKkT​)+Kk​RKkT​=Pk−​−Kk​HPK−​−Pk−​HTKkT​+Kk​HPk−​HTKkT​=Kk​PKkT​

$$K_{k}H{P}_k^{-}tr(P_k)=tr(P_k^{-})-2tr(K_{k}H{P}_k^{-})+tr(K_{k}H{P}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)$$

$$\frac{dtr(P_k)}{d{K}_k}=0-2(H{P}_K^{-}{)}^T+2K_{k}H{P}_k^{-}{H}^T+2K_{k}R$$
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$