卡尔曼滤波器python代码 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^-=AP_{k-1}A^T+Q, P_k^- \in R^{8,8}$

更新阶段

$K_k=\frac{P_k^-C^T}{CP_k^-C^T+R}, K_k\in R^{8,4}$
$\hat{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_kC)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 \tag{1}$
• 观测方程：$z_t=H_t \cdot x_t+v_t \tag{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= \begin{bmatrix} s_t\\ v_t\\ \end{bmatrix} \tag{3}$
其中,$s_t$为t时刻的位移，$v_t$为t时刻的速度

$\begin{cases} s_t& =s_{t-1}+v_t\cdot \vartriangle t+\frac{1}{2}\cdot u_t\cdot \vartriangle t ^2\\ v_t& = v_{t-1} + u_t\cdot \vartriangle t \tag{4} \end{cases}$

写成矩阵的形式
$\begin{bmatrix} s_t\\ v_t\\ \end{bmatrix}= \begin{bmatrix} 1&\vartriangle t\\ 0&1\\ \end{bmatrix} \begin{bmatrix} s_{t-1}\\ v_{t-1}\\ \end{bmatrix}+ \begin{bmatrix} \frac{\vartriangle t ^2}{2}\\ \vartriangle t\\ \end{bmatrix}\cdot u_t \tag{5}$
跟之前的运动方程对比，就知道
$F_t = \begin{bmatrix} 1&\vartriangle t\\ 0&1\\ \end{bmatrix},B_t= \begin{bmatrix} \frac{\vartriangle t ^2}{2}\\ \vartriangle t\\ \end{bmatrix}$
上式就写为
$\hat{x}_{t|t-1}=F_t\cdot\hat{x}_{t-1}+B_t\cdot u_t \tag{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]= \begin{bmatrix} 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}\\ \vdots&\vdots&\vdots&\vdots&\vdots\\ x_{m,1}&x_{m,2}&\cdots&x_{1,m-1}&x_{1,m}\\ \end{bmatrix}\in R^{m,n}$
也就是说$x_t$中包含了n个状态量，并且每个状态量是一个m维向量，也就是存住了t个时刻的量。
还需要注意一点的是，且
$\hat{x}_{t|t-1}$为t时刻的状态矩阵$x_t$ 中不同状态量的均值。且
$\hat{x}_{t|t-1}= \begin{bmatrix} mean(x_1)\\ mean(x_2)\\ \vdots\\ mean(x_n)\\ \end{bmatrix}$
这也好理解，因为$x_t$中应当是真实值，但是真实值事实上永远不可能知道的。不过呢，真实值的均值可以通过计算$\hat{x}_{t|t-1}$得到，并且在均值的附近有误差，也就是一个在均值附近是一个高斯分布。那么接下来求矩阵$x_t$的协方差矩阵就好理解了。

$\begin{equation} \begin{aligned} 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{aligned} \tag{} \end{equation}$
其中 $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{equation} \begin{aligned} 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]\\ &=FP_{t-1}F^T+Q_t \end{aligned} \tag{} \end{equation}$

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

现在我们有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= \begin{bmatrix} z_1\\ z_2\\ \vdots\\ z_n\\ \end{bmatrix}$
$x_t$与$z_t$ 之间由于单位不同，因此需要使用一个转化矩阵H，即
$z_t=H\cdot x_t$写成矩阵形式就是
$\begin{bmatrix} z_1\\ z_2\\ \vdots\\ z_k\\ \end{bmatrix}= H\cdot \begin{bmatrix} x_{1}\\ x_{2}\\ \vdots\\ x_{n}\\ \end{bmatrix}$