wordpress注册直接显示密码,廊坊网站seo服务,昆明做网站词排名优化,html静态网页首页模板本文是看了DeepSORT方法视频之后#xff0c;关于其中使用的卡尔曼滤波的理解
DeepSORT视频链接
首先贴几个比较好的#xff0c;与本文由有关的几个帖子 图说卡尔曼滤波#xff0c;一份通俗易懂的教程 卡尔曼滤波#xff08;Kalman Filter#xff09;原理与公式推导 卡尔…本文是看了DeepSORT方法视频之后关于其中使用的卡尔曼滤波的理解
DeepSORT视频链接
首先贴几个比较好的与本文由有关的几个帖子 图说卡尔曼滤波一份通俗易懂的教程 卡尔曼滤波Kalman Filter原理与公式推导 卡尔曼滤波从入门到精通 协方差的计算X,Y是随机变量,A,B是常数矩阵,如何证明cov(AX,BY)Acov(X,Y)B’ 协方差的计算方法 矩阵求导 两个高斯分布乘积的理论推导 首先是视频中的一张图
预测阶段 x ^ k − A x ^ k − 1 \hat{x}_k^-A\hat{x}_{k-1} x^k−Ax^k−1 P k − A P k − 1 A T Q , P k − ∈ R 8 , 8 P_k^-AP_{k-1}A^TQ, P_k^- \in R^{8,8} Pk−APk−1ATQ,Pk−∈R8,8
更新阶段 K k P k − C T C P k − C T R , K k ∈ R 8 , 4 K_k\frac{P_k^-C^T}{CP_k^-C^TR}, K_k\in R^{8,4} KkCPk−CTRPk−CT,Kk∈R8,4 x k ^ x ^ k − K k ( y k − C x ^ k − ) , C ∈ R 4 , 8 , x ^ k − ∈ R 8 , 1 , y k ∈ R 4 , 1 \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} xk^x^k−Kk(yk−Cx^k−),C∈R4,8,x^k−∈R8,1,yk∈R4,1 P k ( I − K k C ) P k − P_k(I-K_kC)P_k^- Pk(I−KkC)Pk−
整个过程中,矩阵A和矩阵C保持不变具体如下所示。C是状态观测矩阵比如如果我们现在的观测值是速度而需要的是位置那么C就是由速度变化到位置的变换矩阵。而在这里C是由检测框变换到检测框的变换矩阵因此C里都是1 详细步骤
1.获得第一帧输出的检测框参数初始化 x ^ k − \hat{x}_k^- x^k−和 P k − P_k^- Pk−首先被初始化 x ^ 0 − [ x , y , r , h , 0 , 0 , 0 , 0 ] , ∈ R 1 , 8 \hat{x}_0^-[x,y,r,h,0,0,0,0], \in R^{1,8} x^0−[x,y,r,h,0,0,0,0],∈R1,8 P k − P_k^- Pk−与 x ^ 0 − ∈ R 8 , 8 \hat{x}_0^- \in R^{8,8} x^0−∈R8,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过程 x ^ k − \hat{x}_k^- x^k−正常计算 P k − 中的 Q P_k^-中的 Q Pk−中的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_cov3.完成配对给每一个轨迹匹配一个检测框
4.更新过程(Update)
def project(self, mean, covariance): Project state distribution to measurement space. Parameters ---------- mean : ndarray The states mean vector (8 dimensional array). covariance : ndarray The states 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_covdef update(self, mean, covariance, measurement): Run Kalman filter correction step. Parameters ---------- mean : ndarray The predicted states mean vector (8 dimensional). covariance : ndarray The states 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) #求解AXb中的xchol_factor, lower scipy.linalg.cho_factor(projected_cov, lowerTrue, check_finiteFalse) kalman_gain scipy.linalg.cho_solve((chol_factor,lower), np.dot(covariance, self._update_mat.T).T, check_finiteFalse).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 ⋅ x t − 1 B t ⋅ u t ω t (1) x_tF_t \cdot x_{t-1}B_t\cdot u_t\omega_t \tag{1} xtFt⋅xt−1Bt⋅utωt(1)观测方程 z t H t ⋅ x t v t (2) z_tH_t \cdot x_tv_t \tag{2} ztHt⋅xtvt(2)
其中
x t x_t xt为 t t t 时刻的状态向量包括了相机位姿、路标坐标等信息也可能有速度、朝向等信息 u t u_t ut为运动测量值如加速度转向等等 F t F_t Ft为状态转换方程将 t − 1 t-1 t−1 时刻的状态转换至 t t t 时刻的状态 B t B_t Bt 是控制输入矩阵将运动测量值 的作用映射到状态向量上 ω t \omega_t ωt是预测的高斯噪声其均值为0协方差矩阵为 Q t Q_t Qt 。
z t z_t zt为传感器的测量值 H t H_t Ht为转换矩阵它将状态向量映射到测量值所在的空间中由于估计值和预测值可能不同单位也不同因此需要 H t H_t Ht来进行变换。 v t v_t vt为测量的高斯噪声其均值为0协方差矩阵为 R t R_t Rt。
一个小例子: 用一个在解释卡尔曼滤波时最常用的一维例子小车追踪。如下图所示 状态向量 x t x_t xt为小车的位置和速度 x t [ s t v t ] (3) x_t \begin{bmatrix} s_t\\ v_t\\ \end{bmatrix} \tag{3} xt[stvt](3) 其中, s t s_t st为t时刻的位移 v t v_t vt为t时刻的速度 { s t s t − 1 v t ⋅ △ t 1 2 ⋅ u t ⋅ △ t 2 v t v t − 1 u t ⋅ △ t (4) \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} {stvtst−1vt⋅△t21⋅ut⋅△t2vt−1ut⋅△t(4)
写成矩阵的形式 [ s t v t ] [ 1 △ t 0 1 ] [ s t − 1 v t − 1 ] [ △ t 2 2 △ t ] ⋅ u t (5) \begin{bmatrix} s_t\\ v_t\\ \end{bmatrix} \begin{bmatrix} 1\vartriangle t\\ 01\\ \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} [stvt][10△t1][st−1vt−1][2△t2△t]⋅ut(5) 跟之前的运动方程对比就知道 F t [ 1 △ t 0 1 ] , B t [ △ t 2 2 △ t ] F_t \begin{bmatrix} 1\vartriangle t\\ 01\\ \end{bmatrix},B_t \begin{bmatrix} \frac{\vartriangle t ^2}{2}\\ \vartriangle t\\ \end{bmatrix} Ft[10△t1],Bt[2△t2△t] 上式就写为 x ^ t ∣ t − 1 F t ⋅ x ^ t − 1 B t ⋅ u t (6) \hat{x}_{t|t-1}F_t\cdot\hat{x}_{t-1}B_t\cdot u_t \tag{6} x^t∣t−1Ft⋅x^t−1Bt⋅ut(6) 与公式(1)的不同是公式1中的值 x t x_t xt都是真实值因此其中包含有误差而公式6中的 x ^ t ∣ t − 1 \hat{x}_{t|t-1} x^t∣t−1是由运动学方程计算出来的因此其中不包含误差。 联立公式1和6可得 x t − x ^ t ∣ t − 1 F ⋅ ( x t − 1 − x ^ t ∣ t − 1 ) ω t x_t-\hat{x}_{t|t-1}F\cdot (x_{t-1}-\hat{x}_{t|t-1})\omega_t xt−x^t∣t−1F⋅(xt−1−x^t∣t−1)ωt 接下来计算真实值 x t x_t xt的协方差矩阵首先明确一点矩阵 x t x_t xt是一个矩阵它的形式如下所示 x t [ x 1 T , x 2 T , ⋯ , x n T ] [ x 1 , 1 x 1 , 2 ⋯ x 1 , n − 1 x 1 , n x 2 , 1 x 2 , 2 ⋯ x 2 , n − 1 x 2 , n ⋮ ⋮ ⋮ ⋮ ⋮ x m , 1 x m , 2 ⋯ x 1 , m − 1 x 1 , m ] ∈ R m , n x_t[x_1^T,x_2^T,\cdots,x_n^T] \begin{bmatrix} x_{1,1}x_{1,2}\cdotsx_{1,n-1}x_{1,n}\\ x_{2,1}x_{2,2}\cdotsx_{2,n-1}x_{2,n}\\ \vdots\vdots\vdots\vdots\vdots\\ x_{m,1}x_{m,2}\cdotsx_{1,m-1}x_{1,m}\\ \end{bmatrix}\in R^{m,n} xt[x1T,x2T,⋯,xnT] x1,1x2,1⋮xm,1x1,2x2,2⋮xm,2⋯⋯⋮⋯x1,n−1x2,n−1⋮x1,m−1x1,nx2,n⋮x1,m ∈Rm,n 也就是说 x t x_t xt中包含了n个状态量并且每个状态量是一个m维向量也就是存住了t个时刻的量。 还需要注意一点的是且 x ^ t ∣ t − 1 \hat{x}_{t|t-1} x^t∣t−1为t时刻的状态矩阵 x t x_t xt 中不同状态量的均值。且 x ^ t ∣ t − 1 [ m e a n ( x 1 ) m e a n ( x 2 ) ⋮ m e a n ( x n ) ] \hat{x}_{t|t-1} \begin{bmatrix} mean(x_1)\\ mean(x_2)\\ \vdots\\ mean(x_n)\\ \end{bmatrix} x^t∣t−1 mean(x1)mean(x2)⋮mean(xn) 这也好理解因为 x t x_t xt中应当是真实值但是真实值事实上永远不可能知道的。不过呢真实值的均值可以通过计算 x ^ t ∣ t − 1 \hat{x}_{t|t-1} x^t∣t−1得到并且在均值的附近有误差也就是一个在均值附近是一个高斯分布。那么接下来求矩阵 x t x_t xt的协方差矩阵就好理解了。 P t ∣ t − 1 E [ ( x t − x ^ t ∣ t − 1 ) ( x t − x ^ t ∣ t − 1 ) T ] E [ ( F ( x t − x ^ t ∣ t − 1 ) ω t ) ⋅ ( F ( x t − x ^ t ∣ t − 1 ) ω t ) T ] F E [ ( x t − x ^ t ∣ t − 1 ) ⋅ ( x t − x ^ t ∣ t − 1 ) T ] F T E [ F ( x t − x ^ t ∣ t − 1 ) ⋅ ω t T ] E [ ω t ⋅ ( F ( x t − x ^ t ∣ t − 1 ) ) T ] E [ ω t ⋅ ω t 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} Pt∣t−1E[(xt−x^t∣t−1)(xt−x^t∣t−1)T]E[(F(xt−x^t∣t−1)ωt)⋅(F(xt−x^t∣t−1)ωt)T]FE[(xt−x^t∣t−1)⋅(xt−x^t∣t−1)T]FTE[F(xt−x^t∣t−1)⋅ωtT]E[ωt⋅(F(xt−x^t∣t−1))T]E[ωt⋅ωtT]() 其中 E [ F ( x t − x ^ t ∣ t − 1 ) ⋅ ω t T ] E[F(x_t-\hat{x}_{t|t-1})\cdot \omega_t^T] E[F(xt−x^t∣t−1)⋅ωtT]表示矩阵 F ( x t − x ^ t ∣ t − 1 ) F(x_t-\hat{x}_{t|t-1}) F(xt−x^t∣t−1) 与 ω t T \omega_t^T ωtT矩阵的协方差且由于这两者这件并无关系所以 E [ F ( x t − x ^ t ∣ t − 1 ) ⋅ ω t T ] 0 E[F(x_t-\hat{x}_{t|t-1})\cdot \omega_t^T] 0 E[F(xt−x^t∣t−1)⋅ωtT]0同理 E [ ω t ⋅ ( F ( x t − x ^ t ∣ t − 1 ) ) T ] 0 E[\omega_t\cdot (F(x_t-\hat{x}_{t|t-1}))^T]0 E[ωt⋅(F(xt−x^t∣t−1))T]0 注意公式中的E表示的是期望这里是由于协方差的计算方式不同在matlab中的计算公式课本上的有所不同这里知道就可以了。 因此就可以得到协方差的预测公式 P t ∣ t − 1 F E [ ( x t − x ^ t ∣ t − 1 ) ⋅ ( x t − x ^ t ∣ t − 1 ) T ] F E [ ω t ⋅ ω t T ] F P t − 1 F T Q t \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]FE[\omega_t \cdot \omega_t^T]\\ FP_{t-1}F^TQ_t \end{aligned} \tag{} \end{equation} Pt∣t−1FE[(xt−x^t∣t−1)⋅(xt−x^t∣t−1)T]FE[ωt⋅ωtT]FPt−1FTQt()
由以上的步骤我们就得到了预测值和预测值的协方差矩阵接下来就需要将预测值与观测值进行融合了。由于预测值是符合高斯分布观测值也符合高斯分布那么融合的本质就是将这个两个高斯分布乘起来乘起来还是一个高斯分布那么乘起来之后的高斯分布的均值和方差的公式推导见帖子两个高斯分布乘积的理论推导
现在我们有n个预测量假设有k个观测量为 x t − x ^ t ∣ t − 1 F ⋅ ( x t − 1 − x ^ t ∣ t − 1 ) ω t x_t-\hat{x}_{t|t-1}F\cdot (x_{t-1}-\hat{x}_{t|t-1})\omega_t xt−x^t∣t−1F⋅(xt−1−x^t∣t−1)ωt 接下来计算真实值 x t x_t xt的协方差矩阵首先明确一点矩阵 x t x_t xt是一个矩阵它的形式如下所示 z t [ z 1 z 2 ⋮ z n ] z_t \begin{bmatrix} z_1\\ z_2\\ \vdots\\ z_n\\ \end{bmatrix} zt z1z2⋮zn x t x_t xt与 z t z_t zt 之间由于单位不同因此需要使用一个转化矩阵H即 z t H ⋅ x t z_tH\cdot x_t ztH⋅xt写成矩阵形式就是 [ z 1 z 2 ⋮ z k ] H ⋅ [ x 1 x 2 ⋮ x n ] \begin{bmatrix} z_1\\ z_2\\ \vdots\\ z_k\\ \end{bmatrix} H\cdot \begin{bmatrix} x_{1}\\ x_{2}\\ \vdots\\ x_{n}\\ \end{bmatrix} z1z2⋮zk H⋅ x1x2⋮xn
