DeepSORT中的卡尔曼滤波

本文是看了DeepSORT方法视频之后，关于其中使用的卡尔曼滤波的理解

DeepSORT视频链接

首先贴几个比较好的，与本文由有关的几个帖子
图说卡尔曼滤波，一份通俗易懂的教程
卡尔曼滤波（Kalman Filter）原理与公式推导
卡尔曼滤波（Kalman Filter）原理与公式推导2
卡尔曼滤波：从入门到精通
particle filtering—粒子滤波（讲的很通俗易懂）
协方差的计算：X,Y是随机变量,A,B是常数矩阵,如何证明cov(AX,BY)=Acov(X,Y)B’？
协方差的计算方法
矩阵求导
两个高斯分布乘积的理论推导
首先是视频中的一张图

预测阶段

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}$
$P_k^{-}=A{P}_{k-1}{A}^T+Q,P_k^{-}\in R^{8,8}$

更新阶段

$K_k=\frac{P_k^{-}{C}^T}{C{P}_k^{-}{C}^T+R},K_k\in R^{8,4}$
$\widehat{x_k}={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-}),C\in R^{4,8},{\hat{x}}_k^{-}\in R^{8,1},y_k\in R^{4,1}$
$P_k=(I-K_{k}C)P_k^{-}$

整个过程中,矩阵A和矩阵C保持不变，具体如下所示。C是状态观测矩阵，比如，如果我们现在的观测值是速度，而需要的是位置，那么C就是由速度变化到位置的变换矩阵。而在这里，C是由检测框变换到检测框的变换矩阵，因此C里都是1
详细步骤：

1.获得第一帧输出的检测框参数初始化

${\hat{x}}_k^{-}$和$P_k^{-}$首先被初始化
${\hat{x}}_0^{-}=[x,y,r,h,0,0,0,0],\in R^{1,8}$
$P_k^{-}$与${\hat{x}}_0^{-}，\in R^{8,8}$ 有关，差了一个系数，代码如下所示

# self._std_weight_position = 0.05
# self._std_weight_velocity = 0.00625
std = [2 * self._std_weight_position * measurement[3],   #
       2 * self._std_weight_position * measurement[3],    
       1e-2,    
       2 * self._std_weight_position * measurement[3],     
      10 * self._std_weight_velocity * measurement[3],    
      10 * self._std_weight_velocity * measurement[3],   
      1e-5,    
      10 * self._std_weight_velocity * measurement[3]] 
covariance = np.diag(np.square(std))

2.预测下一时刻（第二帧中检测框的位置，图中的Prediction过程）

${\hat{x}}_k^{-}$正常计算，
$P_k^{-}中的Q$是一个随机噪声，其为

std_pos = [ self._std_weight_position * mean[3],     
            self._std_weight_position * mean[3],   
            1e-2,    
            self._std_weight_position * mean[3]] 
 std_vel = [self._std_weight_velocity * mean[3],    
            self._std_weight_velocity * mean[3],    
            1e-5,    
            self._std_weight_velocity * mean[3]] 
  motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))  
  mean = np.dot(self._motion_mat, mean)
  covariance = np.linalg.multi_dot(( self._motion_mat, covariance, self._motion_mat.T)) + motion_cov

3.完成配对，给每一个轨迹匹配一个检测框

4.更新过程(Update)

def project(self, mean, covariance):     
	"""Project state distribution to measurement space.      
	Parameters     
	----------     
	mean : ndarray         The state's mean vector (8 dimensional array).     
	covariance : ndarray         The state's covariance matrix (8x8 dimensional).      
	Returns     
	-------     
	(ndarray, ndarray)         Returns the projected mean and covariance matrix of the given state         estimate.      
	"""     
	std = [ self._std_weight_position * mean[3],        
	        self._std_weight_position * mean[3],        
	        1e-1,         
	        self._std_weight_position * mean[3]]    
	innovation_cov = np.diag(np.square(std))      
	mean = np.dot(self._update_mat, mean)     
	covariance = np.linalg.multi_dot((  self._update_mat, covariance, self._update_mat.T))     
	return mean, covariance + innovation_cov

def update(self, mean, covariance, measurement):    
	 """Run Kalman filter correction step.      
	 Parameters     
	 ----------     
	 mean : ndarray         The predicted state's mean vector (8 dimensional).     covariance : ndarray         The state's covariance matrix (8x8 dimensional).     
	 measurement : ndarray         The 4 dimensional measurement vector (x, y, a, h), where (x, y)         is the center position, a the aspect ratio, and h the height of the         bounding box.     
	  Returns    
	   -------     
	   (ndarray, ndarray)         
	   Returns the measurement-corrected state distribution.     
	    """     
	 projected_mean, projected_cov = self.project(mean, covariance)      
	 #求解AX=b中的x
	 chol_factor, lower = scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)     
	 kalman_gain = scipy.linalg.cho_solve((chol_factor,lower), np.dot(covariance, self._update_mat.T).T,         check_finite=False).T     
	 innovation = measurement - projected_mean      
	 new_mean = mean + np.dot(innovation, kalman_gain.T)     
	 new_covariance = covariance - np.linalg.multi_dot((         
	 kalman_gain, projected_cov, kalman_gain.T))     
	 return new_mean, new_covariance

本文在卡尔曼滤波：从入门到精通的基础上，又添加了一些个人的理解

导论

卡尔曼滤波本质上是一个数据融合算法，将具有同样测量目的、来自不同传感器、(可能) 具有不同单位 (unit) 的数据融合在一起，得到一个更精确的目的测量值。事实上，卡尔曼滤波是将两个高斯分布相乘而得到的一个新的高斯分布。

简述

首先考虑一个SLAM问题

• 运动方程：$$x_t=F_t\cdot x_{t-1}+B_t\cdot u_t+{\omega }_t\text{\hspace{1em}(1)}$$
• 观测方程：$$z_t=H_t\cdot x_t+v_t\text{\hspace{1em}(2)}$$

其中：

​ $x_t$为$t$ 时刻的状态向量，包括了相机位姿、路标坐标等信息，也可能有速度、朝向等信息；
​ $u_t$为运动测量值，如加速度，转向等等；
​ $F_t$为状态转换方程，将$t-1$ 时刻的状态转换至$t$ 时刻的状态；
​ $B_t$ 是控制输入矩阵，将运动测量值​ 的作用映射到状态向量上；
​ ${\omega }_t$是预测的高斯噪声，其均值为0，协方差矩阵为$Q_t$​ 。

​ $z_t$为传感器的测量值；
​ $H_t$为转换矩阵，它将状态向量映射到测量值所在的空间中，由于估计值和预测值可能不同，单位也不同，因此需要$H_t$来进行变换。
​ $v_t$为测量的高斯噪声，其均值为0，协方差矩阵为​ $R_t$。

一个小例子:
用一个在解释卡尔曼滤波时最常用的一维例子：小车追踪。如下图所示：

状态向量$x_t$为小车的位置和速度：

$$x_t=\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]\text{\hspace{1em}(3)}$$
其中,$s_t$为t时刻的位移，$v_t$为t时刻的速度

$$\{\begin{array}{ll}{s}_t& =s_{t-1}+v_t\cdot △t+\frac{1}{2}\cdot u_t\cdot △t^2\\ v_t& =v_{t-1}+u_t\cdot △t\end{array}\text{\hspace{1em}(4)}$$

写成矩阵的形式
$$\left[\begin{array}{c}{s}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& △t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{s}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{△t^2}{2}\\ △t\end{array}\right]\cdot u_t\text{\hspace{1em}(5)}$$
跟之前的运动方程对比，就知道
$$F_t=\left[\begin{array}{cc}1& △t\\ 0& 1\end{array}\right],B_t=\left[\begin{array}{c}\frac{△t^2}{2}\\ △t\end{array}\right]$$
上式就写为
$${\hat{x}}_{t|t-1}=F_t\cdot {\hat{x}}_{t-1}+B_t\cdot u_t\text{\hspace{1em}(6)}$$
与公式(1)的不同是，公式（1）中的值$x_t$都是真实值，因此其中包含有误差，而公式（6）中的 ${\hat{x}}_{t|t-1}$是由运动学方程计算出来的，因此其中不包含误差。
联立公式（1）和（6）可得：
$$x_t-{\hat{x}}_{t|t-1}=F\cdot (x_{t-1}-{\hat{x}}_{t|t-1})+{\omega }_t$$
接下来计算真实值$x_t$的协方差矩阵，首先明确一点矩阵$x_t$是一个矩阵，它的形式如下所示：
$$x_t=[x_1^T,x_2^T,\cdots ,x_n^T]=\left[\begin{array}{ccccc}{x}_{1,1}& x_{1,2}& \cdots & x_{1,n-1}& x_{1,n}\\ x_{2,1}& x_{2,2}& \cdots & x_{2,n-1}& x_{2,n}\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ x_{m,1}& x_{m,2}& \cdots & x_{1,m-1}& x_{1,m}\end{array}\right]\in R^{m,n}$$
也就是说$x_t$中包含了n个状态量，并且每个状态量是一个m维向量，也就是存住了t个时刻的量。
还需要注意一点的是，且
${\hat{x}}_{t|t-1}$为t时刻的状态矩阵$x_t$ 中不同状态量的均值。且
$${\hat{x}}_{t|t-1}=\left[\begin{array}{c}mean(x_1)\\ mean(x_2)\\ ⋮\\ mean(x_n)\end{array}\right]$$
这也好理解，因为$x_t$中应当是真实值，但是真实值事实上永远不可能知道的。不过呢，真实值的均值可以通过计算${\hat{x}}_{t|t-1}$得到，并且在均值的附近有误差，也就是一个在均值附近是一个高斯分布。那么接下来求矩阵$x_t$的协方差矩阵就好理解了。

$$\begin{array}{c}\begin{array}{rl}{P}_{t|t-1}& =E[(x_t-{\hat{x}}_{t|t-1})(x_t-{\hat{x}}_{t|t-1}{)}^T]\\ & =E[(F(x_t-{\hat{x}}_{t|t-1})+{\omega }_t)\cdot (F(x_t-{\hat{x}}_{t|t-1})+{\omega }_t{)}^T]\\ & =FE[(x_t-{\hat{x}}_{t|t-1})\cdot (x_t-{\hat{x}}_{t|t-1}{)}^T]F^T\\ & +E[F(x_t-{\hat{x}}_{t|t-1})\cdot {\omega }_t^T]+E[{\omega }_t\cdot (F(x_t-{\hat{x}}_{t|t-1}){)}^T]\\ & +E[{\omega }_t\cdot {\omega }_t^T]\end{array}\end{array}\text{\hspace{1em}()}$$
其中 $E[F(x_t-{\hat{x}}_{t|t-1})\cdot {\omega }_t^T]$表示矩阵$F(x_t-{\hat{x}}_{t|t-1})$ 与${\omega }_t^T$矩阵的协方差，且由于这两者这件并无关系，所以
$$E[F(x_t-{\hat{x}}_{t|t-1})\cdot {\omega }_t^T]=0$$同理
$$E[{\omega }_t\cdot (F(x_t-{\hat{x}}_{t|t-1}){)}^T]=0$$
注意公式中的E表示的是期望，这里是由于协方差的计算方式不同，在matlab中的计算公式课本上的有所不同，这里知道就可以了。
因此就可以得到协方差的预测公式
$$\begin{array}{c}\begin{array}{rl}{P}_{t|t-1}& =FE[(x_t-{\hat{x}}_{t|t-1})\cdot (x_t-{\hat{x}}_{t|t-1}{)}^T]F+E[{\omega }_t\cdot {\omega }_t^T]\\ & =F{P}_{t-1}{F}^T+Q_t\end{array}\end{array}\text{\hspace{1em}()}$$

由以上的步骤，我们就得到了预测值和预测值的协方差矩阵，接下来就需要将预测值与观测值进行融合了。由于预测值是符合高斯分布，观测值也符合高斯分布，那么融合的本质就是将这个两个高斯分布乘起来，乘起来还是一个高斯分布，那么乘起来之后的高斯分布的均值和方差的公式推导，见帖子两个高斯分布乘积的理论推导

现在我们有n个预测量，假设有k个观测量为
$$x_t-{\hat{x}}_{t|t-1}=F\cdot (x_{t-1}-{\hat{x}}_{t|t-1})+{\omega }_t$$
接下来计算真实值$x_t$的协方差矩阵，首先明确一点矩阵$x_t$是一个矩阵，它的形式如下所示：

$$z_t=\left[\begin{array}{c}{z}_1\\ z_2\\ ⋮\\ z_n\end{array}\right]$$
$x_t$与$z_t$ 之间由于单位不同，因此需要使用一个转化矩阵H，即
$$z_t=H\cdot x_t$$写成矩阵形式就是
$$\left[\begin{array}{c}{z}_1\\ z_2\\ ⋮\\ z_k\end{array}\right]=H\cdot \left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]$$