SLAM 目标跟踪算法 目标跟踪经典算法

SORT目标追踪

文章目录

• SORT目标追踪
• 前言
• 一、卡尔曼滤波器

• 1.迭代算法
• 2.数据融合
• 3.协方差矩阵
• 4.状态空间表达
• 5.卡尔曼滤波详细推导

---

前言

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

---

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

一、卡尔曼滤波器

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

1.迭代算法

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

---

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

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

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

也就是当前的估计值 = 上一时刻的估计值 + 系数 * （当前时刻测量值 - 上一次的估计值）
此时的系数即为Kalman Gain，卡尔曼增益/因数
这样迭代求估计值的过程为：
①计算Kalman Gain $K_k = \frac{ e_{EST_{k-1}} } { e_{EST_{k-1}} + e_{MEA_{k}} }$
②计算先验估计值 $\hat{x_{k}}=\hat{x}_{k-1}+K_k(Z_k-\hat{x}_{k-1})$
③更新估计误差 $e_{EST_{k}}=(1-K_k)e_{EST_{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使得方差最小
$\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$
令 $\frac{\delta^2}{k}=0$，解得：
$k=\frac{{\delta_1}^2}{ {\delta_1}^2+ {\delta_1}^2} = \frac{2*2}{2*2+4*4}=0.2$
$\hat{z}=30+0.2*(32-30)=30.4$
$\delta_{\hat{z}}^2=(1-0.2)*(1-0.2)*2*2+0.2*0.2*4*4=3.2$

3.协方差矩阵

协方差矩阵就是将方差与协方差在一个矩阵中表现出来，表示变量间的联动关系
协方差$\delta_x\delta_y$表示两个变量的变化方向，大于0表示正相关
一般协方差矩阵表示为
$\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}$
接下来介绍一个求解协方差矩阵的方法，方便编程计算协方差矩阵
引入过渡矩阵a
$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}$
$P = \frac{1}{3}a^Ta$

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}Bx_2-\frac{1}{m}kx_1$

$\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$

$\begin{bmatrix} z_1 \\ z_2 \end{bmatrix} = \begin{bmatrix} 1&0 \\ 0&1 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2\end{bmatrix}$

离散化之后，可以总结为：

$X_k=AX_{k-1}+Bu+w_{k-1}$ 过程噪音

$Z_k=HX_k+v_k$

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

5.卡尔曼滤波详细推导

状态空间方程：
$X_k=AX_{k-1}+BU_{k-1}+W_{k-1} \\ Z_k=HX_k+V_k$
但是$w_{k-1}$和$v_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 ②$
$\hat{x}$表示$x$的估计值，$\hat{x}^{-}$表示$x$的先验估计值，只通过状态空间方程得到，没有经过任何处理
这样，我们得到一个算出来的结果$\hat{X_k^-}$和一个测出来的结果$Z_k$，这两个数值都是不准确的，那么怎么得到一个较为精确的数值呢。自然想到我们上面介绍的数据融合。
$\hat{X_k}=\hat{X_k^{-}}+G(H^{-1}Z_k-\hat{X_k^{-}})$
教科书中一般写成如下公式：
$G=K_kH$
$\hat{X_k}=\hat{X_k^{-}}+K_k(Z_k-H\hat{X_k^{-}})$
这样写应该是为了防止$H$矩阵不可逆吧。
那么我们的目标就是寻找$K_k$，使得$\hat{X_k}$更加接近于$X_k$(真实值)
$e_k = X_k-\hat{X_k}$
$w_{k-1}$是过程噪声，我们假设真实世界的噪声是符合正态分布的，$e_k$满足正态分布，$e_k$的协方差矩阵：
$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}$
$e_k$的期望最小，等价于方差最小，等价于P的迹最小
$(AB)^T=B^TA^T\\(A+B)^T=A^T+B^T$
$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$

$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$

$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)$

$\frac{dtr(P_k)}{dK_k}=0-2(HP_K^-)^T+2K_kHP_k^-H^T+2K_kR$
$K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}$