本文主要是介绍DeepSORT（目标跟踪算法）中卡尔曼滤波器中的预测，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
DeepSORT（目标跟踪算法）中卡尔曼滤波器中的预测
flyfish
先略再详，先简洁的说，再细说。
卡尔曼滤波器的预测步骤主要有两个目标：
- 预测下一时刻的状态向量：根据当前状态和运动模型，预测对象在下一时刻的位置和速度。
- 更新协方差矩阵：根据运动模型和过程噪声，更新预测的状态不确定性。
两个重要的点
- 运动模型：由状态转移矩阵 F \mathbf{F} F 描述，在代码中是 self._motion_mat。
- 过程噪声：由过程噪声协方差矩阵 Q \mathbf{Q} Q 描述，在代码中是 motion_cov。
predict 函数
def predict(self, mean, covariance):"""执行卡尔曼滤波的预测步骤。参数----------mean : ndarray前一时间步长中对象状态的8维均值向量。covariance : ndarray前一时间步长中对象状态的8x8维协方差矩阵。返回-------(ndarray, ndarray)返回预测状态的均值向量和协方差矩阵。未观测到的速度初始化为0均值。"""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_covreturn mean, covariance
预测步骤详解
- 过程噪声的标准差计算：
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]))
- std_pos 和 std_vel 分别是位置和速度的不确定性的标准差。这些标准差基于当前状态中的高度（mean[3]）和预定义的权重系数 _std_weight_position 和 _std_weight_velocity 计算得出。
- motion_cov 是过程噪声的协方差矩阵，表示由于模型的不确定性而引入的噪声。代码中过程噪声协方差矩阵 motion_cov 是一个对角矩阵， motion_cov 是对角矩阵，其中对角线元素是位置和速度标准差的平方，即方差。
这些方差值是根据当前状态中的高度（mean[3]）和预定义的权重系数 _std_weight_position 和 _std_weight_velocity 计算得出的。
- 预测状态均值：
mean = np.dot(self._motion_mat, mean)
- 使用状态转移矩阵 _motion_mat 对当前状态均值 mean 进行线性变换，预测下一时刻的状态均值。
- 预测协方差矩阵：
covariance = np.linalg.multi_dot((self._motion_mat, covariance, self._motion_mat.T)) + motion_cov
- 使用状态转移矩阵 _motion_mat 更新协方差矩阵 covariance，并加上过程噪声的协方差矩阵 motion_cov，以反映由于运动模型和过程噪声引入的不确定性。
预测的内容
- 状态均值（mean）：包含对象的预测位置和速度。这表示根据当前状态和运动模型，预测对象在下一时刻的位置和速度。
- 协方差矩阵（covariance）：描述状态均值的不确定性。通过状态转移矩阵和过程噪声的更新，反映预测的不确定性。
具体理解过程噪声
假设当前状态的高度（mean[3]）为 h h h，过程噪声的标准差为：
-
位置不确定性：_std_weight_position * h
-
速度不确定性：_std_weight_velocity * h
如果这些权重分别为 1 20 \frac{1}{20} 201 和 1 160 \frac{1}{160} 1601，则过程噪声的方差（对角线元素）为： -
位置不确定性方差： ( h 20 ) 2 (\frac{h}{20})^2 (20h)2
-
速度不确定性方差： ( h 160 ) 2 (\frac{h}{160})^2 (160h)2
过程噪声的含义
- 未建模的动态变化：
- 系统中可能存在某些动态变化，未在运动模型中被完全描述。例如，在追踪一个行人时，行人可能会突然改变速度或方向，这种变化是无法通过简单的线性运动模型预测的。
- 外部扰动：
- 过程噪声也可以包括由于外部环境引起的扰动。例如，风、地形变化或其他物体的干扰都会影响对象的实际运动。
- 系统内在的不确定性：
- 即使在系统设计时考虑了所有已知的因素，实际系统中仍然存在一些内在的不确定性。例如，机械部件的磨损、传感器的精度限制等。
过程噪声在卡尔曼滤波中的作用
在卡尔曼滤波器的预测步骤中，过程噪声被加到预测的协方差矩阵上，用于反映由于这些不确定性引入的额外误差。它使得卡尔曼滤波器在更新步骤中对新观测数据的调整更为灵活。
综述
- 过程噪声：代表系统在预测时的内在不确定性和外部扰动，描述了模型预测误差的来源。
- 作用：在卡尔曼滤波的预测步骤中，通过增加协方差矩阵，反映预测的额外不确定性，使得滤波器更灵活地适应新观测数据。
- 计算：在代码中通过标准差计算并构建成对角协方差矩阵 motion_cov，反映了位置和速度的过程噪声。
过程噪声是卡尔曼滤波器的重要组成部分，确保了滤波器在面对现实世界的不确定性时具有鲁棒性。
状态均值预测公式
状态均值预测公式为：
x k ∣ k − 1 = F x k − 1 ∣ k − 1 \mathbf{x}_{k|k-1} = \mathbf{F} \mathbf{x}_{k-1|k-1} xk∣k−1=Fxk−1∣k−1
（这里不理解，没问题，因为下面会将公式展开，就容易理解了）
在代码中，这一部分实现为：
mean = np.dot(self._motion_mat, mean)
具体展开成详细的形式是：
假设当前状态均值向量 x k − 1 ∣ k − 1 \mathbf{x}_{k-1|k-1} xk−1∣k−1 为：
x k − 1 ∣ k − 1 = [ x y a h v x v y v a v h ] \mathbf{x}_{k-1|k-1} = \begin{bmatrix} x \\ y \\ a \\ h \\ vx \\ vy \\ va \\ vh \end{bmatrix} xk−1∣k−1= xyahvxvyvavh
状态转移矩阵 F \mathbf{F} F 为：
[ 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 ] \begin{bmatrix}1 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} 1000000001000000001000000001000010001000010001000010001000010001
那么，预测的状态均值 x k ∣ k − 1 \mathbf{x}_{k|k-1} xk∣k−1为：
x k ∣ k − 1 = F x k − 1 ∣ k − 1 = [ 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 ] [ x y a h v x v y v a v h ] \mathbf{x}_{k|k-1} = \mathbf{F} \mathbf{x}_{k-1|k-1} = \begin{bmatrix} 1 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ a \\ h \\ vx \\ vy \\ va \\ vh \end{bmatrix} xk∣k−1=Fxk−1∣k−1= 1000000001000000001000000001000010001000010001000010001000010001 xyahvxvyvavh
计算结果是：
x k ∣ k − 1 = [ x + v x y + v y a + v a h + v h v x v y v a v h ] \mathbf{x}_{k|k-1} = \begin{bmatrix} x + vx \\ y + vy \\ a + va \\ h + vh \\ vx \\ vy \\ va \\ vh \end{bmatrix} xk∣k−1= x+vxy+vya+vah+vhvxvyvavh
即，新的位置通过加上速度得到，而速度保持不变。
协方差矩阵预测公式
P k ∣ k − 1 = F P k − 1 ∣ k − 1 F ⊤ + Q \mathbf{P}_{k|k-1} = \mathbf{F} \mathbf{P}_{k-1|k-1} \mathbf{F}^\top + \mathbf{Q} Pk∣k−1=FPk−1∣k−1F⊤+Q
在代码中，这一部分实现为：
covariance = np.linalg.multi_dot((
self._motion_mat, covariance, self._motion_mat.T)) + motion_cov
具体展开成详细的形式是：
假设当前协方差矩阵 P k − 1 ∣ k − 1 \mathbf{P}_{k-1|k-1} Pk−1∣k−1为：
P k − 1 ∣ k − 1 = [ P x x P x y P x a P x h P x v x P x v y P x v a P x v h P x y P y y P y a P y h P y v x P y v y P y v a P y v h P x a P y a P a a P a h P a v x P a v y P a v a P a v h P x h P y h P a h P h h P h v x P h v y P h v a P h v h P x v x P y v x P a v x P h v x P v x v x P v x v y P v x v a P v x v h P x v y P y v y P a v y P h v y P v x v y P v y v y P v y v a P v y v h P x v a P y v a P a v a P h v a P v x v a P v y v a P v a v a P v a v h P x v h P y v h P a v h P h v h P v x v h P v y v h P v a v h P v h v h ] \mathbf{P}_{k-1|k-1} = \begin{bmatrix} P_{xx} & P_{xy} & P_{xa} & P_{xh} & P_{xvx} & P_{xvy} & P_{xva} & P_{xvh} \\ P_{xy} & P_{yy} & P_{ya} & P_{yh} & P_{yvx} & P_{yvy} & P_{yva} & P_{yvh} \\ P_{xa} & P_{ya} & P_{aa} & P_{ah} & P_{avx} & P_{avy} & P_{ava} & P_{avh} \\ P_{xh} & P_{yh} & P_{ah} & P_{hh} & P_{hvx} & P_{hvy} & P_{hva} & P_{hvh} \\ P_{xvx} & P_{yvx} & P_{avx} & P_{hvx} & P_{vxvx} & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} & P_{yvy} & P_{avy} & P_{hvy} & P_{vxvy} & P_{vyvy} & P_{vyva} & P_{vyvh} \\ P_{xva} & P_{yva} & P_{ava} & P_{hva} & P_{vxva} & P_{vyva} & P_{vava} & P_{vavh} \\ P_{xvh} & P_{yvh} & P_{avh} & P_{hvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} \end{bmatrix} Pk−1∣k−1= PxxPxyPxaPxhPxvxPxvyPxvaPxvhPxyPyyPyaPyhPyvxPyvyPyvaPyvhPxaPyaPaaPahPavxPavyPavaPavhPxhPyhPahPhhPhvxPhvyPhvaPhvhPxvxPyvxPavxPhvxPvxvxPvxvyPvxvaPvxvhPxvyPyvyPavyPhvyPvxvyPvyvyPvyvaPvyvhPxvaPyvaPavaPhvaPvxvaPvyvaPvavaPvavhPxvhPyvhPavhPhvhPvxvhPvyvhPvavhPvhvh
过程噪声协方差矩阵 Q \mathbf{Q} Q 为：
Q = m o t i o n _ c o v = [ σ x 2 0 0 0 0 0 0 0 0 σ y 2 0 0 0 0 0 0 0 0 σ a 2 0 0 0 0 0 0 0 0 σ h 2 0 0 0 0 0 0 0 0 σ v x 2 0 0 0 0 0 0 0 0 σ v y 2 0 0 0 0 0 0 0 0 σ v a 2 0 0 0 0 0 0 0 0 σ v h 2 ] \mathbf{Q} = {motion\_cov} = \begin{bmatrix} \sigma_{x}^2 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{y}^2 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{a}^2 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{h}^2 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{vx}^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{vy}^2 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{va}^2 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{vh}^2 \end{bmatrix} Q=motion_cov= σx200000000σy200000000σa200000000σh200000000σvx200000000σvy200000000σva200000000σvh2
这个公式表示当前时刻的协方差矩阵通过状态转移矩阵左乘和右乘后，加上过程噪声协方差矩阵，得到下一时刻的协方差矩阵。
我们已经计算了 F P k − 1 ∣ k − 1 \mathbf{F} \mathbf{P}_{k-1|k-1} FPk−1∣k−1 的部分，现在继续计算 F P k − 1 ∣ k − 1 F ⊤ \mathbf{F} \mathbf{P}_{k-1|k-1} \mathbf{F}^\top FPk−1∣k−1F⊤。
计算 ( F P k − 1 ∣ k − 1 ) F ⊤ (\mathbf{F} \mathbf{P}_{k-1|k-1}) \mathbf{F}^\top (FPk−1∣k−1)F⊤
我们之前得到了：
F P k − 1 ∣ k − 1 = [ P x x + P x v x P x y + P x v y P x a + P x v a P x h + P x v h P x v x P x v y P x v a P x v h P x y + P y v x P y y + P y v y P y a + P y v a P y h + P y v h P y v x P y v y P y v a P y v h P x a + P a v x P y a + P a v y P a a + P a v a P a h + P a v h P a v x P a v y P a v a P a v h P x h + P h v x P y h + P h v y P a h + P h v a P h h + P h v h P h v x P h v y P h v a P h v h P x v x P y v x P a v x P h v x P v x v x P v x v y P v x v a P v x v h P x v y P y v y P a v y P h v y P v x v y P v y v y P v y v a P v y v h P x v a P y v a P a v a P h v a P v x v a P v y v a P v a v a P v a v h P x v h P y v h P a v h P h v h P v x v h P v y v h P v a v h P v h v h ] \mathbf{F} \mathbf{P}_{k-1|k-1} = \begin{bmatrix} P_{xx} + P_{xvx} & P_{xy} + P_{xvy} & P_{xa} + P_{xva} & P_{xh} + P_{xvh} & P_{xvx} & P_{xvy} & P_{xva} & P_{xvh} \\ P_{xy} + P_{yvx} & P_{yy} + P_{yvy} & P_{ya} + P_{yva} & P_{yh} + P_{yvh} & P_{yvx} & P_{yvy} & P_{yva} & P_{yvh} \\ P_{xa} + P_{avx} & P_{ya} + P_{avy} & P_{aa} + P_{ava} & P_{ah} + P_{avh} & P_{avx} & P_{avy} & P_{ava} & P_{avh} \\ P_{xh} + P_{hvx} & P_{yh} + P_{hvy} & P_{ah} + P_{hva} & P_{hh} + P_{hvh} & P_{hvx} & P_{hvy} & P_{hva} & P_{hvh} \\ P_{xvx} & P_{yvx} & P_{avx} & P_{hvx} & P_{vxvx} & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} & P_{yvy} & P_{avy} & P_{hvy} & P_{vxvy} & P_{vyvy} & P_{vyva} & P_{vyvh} \\ P_{xva} & P_{yva} & P_{ava} & P_{hva} & P_{vxva} & P_{vyva} & P_{vava} & P_{vavh} \\ P_{xvh} & P_{yvh} & P_{avh} & P_{hvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} \end{bmatrix} FPk−1∣k−1= Pxx+PxvxPxy+PyvxPxa+PavxPxh+PhvxPxvxPxvyPxvaPxvhPxy+PxvyPyy+PyvyPya+PavyPyh+PhvyPyvxPyvyPyvaPyvhPxa+PxvaPya+PyvaPaa+PavaPah+PhvaPavxPavyPavaPavhPxh+PxvhPyh+PyvhPah+PavhPhh+PhvhPhvxPhvyPhvaPhvhPxvxPyvxPavxPhvxPvxvxPvxvyPvxvaPvxvhPxvyPyvyPavyPhvyPvxvyPvyvyPvyvaPvyvhPxvaPyvaPavaPhvaPvxvaPvyvaPvavaPvavhPxvhPyvhPavhPhvhPvxvhPvyvhPvavhPvhvh
接下来，计算这个矩阵与 F ⊤ \mathbf{F}^\top F⊤ 的乘积：
F ⊤ = [ 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 ] \mathbf{F}^\top = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 1 \end{bmatrix} F⊤= 1000100001000100001000100001000100001000000001000000001000000001
进行矩阵乘法：
( F P k − 1 ∣ k − 1 ) F ⊤ = [ P x x + P x v x P x y + P x v y P x a + P x v a P x h + P x v h P x v x P x v y P x v a P x v h P x y + P y v x P y y + P y v y P y a + P y v a P y h + P y v h P y v x P y v y P y v a P y v h P x a + P a v x P y a + P a v y P a a + P a v a P a h + P a v h P a v x P a v y P a v a P a v h P x h + P h v x P y h + P h v y P a h + P h v a P h h + P h v h P h v x P h v y P h v a P h v h P x v x P y v x P a v x P h v x P v x v x P v x v y P v x v a P v x v h P x v y P y v y P a v y P h v y P v x v y P v y v y P v y v a P v y v h P x v a P y v a P a v a P h v a P v x v a P v y v a P v a v a P v a v h P x v h P y v h P a v h P h v h P v x v h P v y v h P v a v h P v h v h ] [ 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 ] (\mathbf{F} \mathbf{P}_{k-1|k-1}) \mathbf{F}^\top = \begin{bmatrix} P_{xx} + P_{xvx} & P_{xy} + P_{xvy} & P_{xa} + P_{xva} & P_{xh} + P_{xvh} & P_{xvx} & P_{xvy} & P_{xva} & P_{xvh} \\ P_{xy} + P_{yvx} & P_{yy} + P_{yvy} & P_{ya} + P_{yva} & P_{yh} + P_{yvh} & P_{yvx} & P_{yvy} & P_{yva} & P_{yvh} \\ P_{xa} + P_{avx} & P_{ya} + P_{avy} & P_{aa} + P_{ava} & P_{ah} + P_{avh} & P_{avx} & P_{avy} & P_{ava} & P_{avh} \\ P_{xh} + P_{hvx} & P_{yh} + P_{hvy} & P_{ah} + P_{hva} & P_{hh} + P_{hvh} & P_{hvx} & P_{hvy} & P_{hva} & P_{hvh} \\ P_{xvx} & P_{yvx} & P_{avx} & P_{hvx} & P_{vxvx} & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} & P_{yvy} & P_{avy} & P_{hvy} & P_{vxvy} & P_{vyvy} & P_{vyva} & P_{vyvh} \\ P_{xva} & P_{yva} & P_{ava} & P_{hva} & P_{vxva} & P_{vyva} & P_{vava} & P_{vavh} \\ P_{xvh} & P_{yvh} & P_{avh} & P_{hvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} (FPk−1∣k−1)F⊤= Pxx+PxvxPxy+PyvxPxa+PavxPxh+PhvxPxvxPxvyPxvaPxvhPxy+PxvyPyy+PyvyPya+PavyPyh+PhvyPyvxPyvyPyvaPyvhPxa+PxvaPya+PyvaPaa+PavaPah+PhvaPavxPavyPavaPavhPxh+PxvhPyh+PyvhPah+PavhPhh+PhvhPhvxPhvyPhvaPhvhPxvxPyvxPavxPhvxPvxvxPvxvyPvxvaPvxvhPxvyPyvyPavyPhvyPvxvyPvyvyPvyvaPvyvhPxvaPyvaPavaPhvaPvxvaPvyvaPvavaPvavhPxvhPyvhPavhPhvhPvxvhPvyvhPvavhPvhvh 1000000001000000001000000001000010001000010001000010001000010001
结果是：
( F P k − 1 ∣ k − 1 ) F ⊤ = [ P x x + 2 P x v x + P v x v x P x y + P x v y + P y v x + P v x v y P x a + P x v a + P a v x + P v x v a P x h + P x v h + P h v x + P v x v h P x v x + P v x v x P x v y + P v x v y P x v a + P v x v a P x v h + P v x v h P x y + P y v x + P x v y + P v x v y P y y + 2 P y v y + P v y v y P y a + P y v a + P a v y + P v y v a P y h + P y v h + P h v y + P v y v h P y v x + P v x v y P y v y + P v y v y P y v a + P v y v a P y v h + P v y v h P x a + P a v x + P x v a + P v x v a P y a + P a v y + P y v a + P v y v a P a a + 2 P a v a + P v a v a P a h + P a v h + P h v a + P v a v h P a v x + P v x v a P a v y + P v y v a P a v a + P v a v a P a v h + P v a v h P x h + P h v x + P x v h + P v x v h P y h + P h v y + P y v h + P v y v h P a h + P h v a + P a v h + P v a v h P h h + 2 P h v h + P v h v h P h v x + P v x v h P h v y + P v y v h P h v a + P v a v h P h v h + P v h v h P x v x + P v x v x P y v x + P v x v y P a v x + P v x v a P h v x + P v x v h P v x v x P v x v y P v x v a P v x v h P x v y + P v x v y P y v y + P v y v y P a v y + P v y v a P h v y + P v y v h P v x v y P v y v y P v y v a P v y v h P x v a + P v x v a P y v a + P v y v a P a v a + P v a v a P h v a + P v a v h P v x v a P v y v a P v a v a P v a v h P x v h + P v x v h P y v h + P v y v h P a v h + P v a v h P h v h + P v h v h P v x v h P v y v h P v a v h P v h v h ] (\mathbf{F} \mathbf{P}_{k-1|k-1}) \mathbf{F}^\top = \begin{bmatrix} P_{xx} + 2P_{xvx} + P_{vxvx} & P_{xy} + P_{xvy} + P_{yvx} + P_{vxvy} & P_{xa} + P_{xva} + P_{avx} + P_{vxva} & P_{xh} + P_{xvh} + P_{hvx} + P_{vxvh} & P_{xvx} + P_{vxvx} & P_{xvy} + P_{vxvy} & P_{xva} + P_{vxva} & P_{xvh} + P_{vxvh} \\ P_{xy} + P_{yvx} + P_{xvy} + P_{vxvy} & P_{yy} + 2P_{yvy} + P_{vyvy} & P_{ya} + P_{yva} + P_{avy} + P_{vyva} & P_{yh} + P_{yvh} + P_{hvy} + P_{vyvh} & P_{yvx} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{yva} + P_{vyva} & P_{yvh} + P_{vyvh} \\ P_{xa} + P_{avx} + P_{xva} + P_{vxva} & P_{ya} + P_{avy} + P_{yva} + P_{vyva} & P_{aa} + 2P_{ava} + P_{vava} & P_{ah} + P_{avh} + P_{hva} + P_{vavh} & P_{avx} + P_{vxva} & P_{avy} + P_{vyva} & P_{ava} + P_{vava} & P_{avh} + P_{vavh} \\ P_{xh} + P_{hvx} + P_{xvh} + P_{vxvh} & P_{yh} + P_{hvy} + P_{yvh} + P_{vyvh} & P_{ah} + P_{hva} + P_{avh} + P_{vavh} & P_{hh} + 2P_{hvh} + P_{vhvh} & P_{hvx} + P_{vxvh} & P_{hvy} + P_{vyvh} & P_{hva} + P_{vavh} & P_{hvh} + P_{vhvh} \\ P_{xvx} + P_{vxvx} & P_{yvx} + P_{vxvy} & P_{avx} + P_{vxva} & P_{hvx} + P_{vxvh} & P_{vxvx} & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{avy} + P_{vyva} & P_{hvy} + P_{vyvh} & P_{vxvy} & P_{vyvy} & P_{vyva} & P_{vyvh} \\ P_{xva} + P_{vxva} & P_{yva} + P_{vyva} & P_{ava} + P_{vava} & P_{hva} + P_{vavh} & P_{vxva} & P_{vyva} & P_{vava} & P_{vavh} \\ P_{xvh} + P_{vxvh} & P_{yvh} + P_{vyvh} & P_{avh} + P_{vavh} & P_{hvh} + P_{vhvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} \end{bmatrix} (FPk−1∣k−1)F⊤= Pxx+2Pxvx+PvxvxPxy+Pyvx+Pxvy+PvxvyPxa+Pavx+Pxva+PvxvaPxh+Phvx+Pxvh+PvxvhPxvx+PvxvxPxvy+PvxvyPxva+PvxvaPxvh+PvxvhPxy+Pxvy+Pyvx+PvxvyPyy+2Pyvy+PvyvyPya+Pavy+Pyva+PvyvaPyh+Phvy+Pyvh+PvyvhPyvx+PvxvyPyvy+PvyvyPyva+PvyvaPyvh+PvyvhPxa+Pxva+Pavx+PvxvaPya+Pyva+Pavy+PvyvaPaa+2Pava+PvavaPah+Phva+Pavh+PvavhPavx+PvxvaPavy+PvyvaPava+PvavaPavh+PvavhPxh+Pxvh+Phvx+PvxvhPyh+Pyvh+Phvy+PvyvhPah+Pavh+Phva+PvavhPhh+2Phvh+PvhvhPhvx+PvxvhPhvy+PvyvhPhva+PvavhPhvh+PvhvhPxvx+PvxvxPyvx+PvxvyPavx+PvxvaPhvx+PvxvhPvxvxPvxvyPvxvaPvxvhPxvy+PvxvyPyvy+PvyvyPavy+PvyvaPhvy+PvyvhPvxvyPvyvyPvyvaPvyvhPxva+PvxvaPyva+PvyvaPava+PvavaPhva+PvavhPvxvaPvyvaPvavaPvavhPxvh+PvxvhPyvh+PvyvhPavh+PvavhPhvh+PvhvhPvxvhPvyvhPvavhPvhvh
最终的预测协方差矩阵
将上面的结果与过程噪声协方差矩阵 Q \mathbf{Q} Q 相加：
P k ∣ k − 1 = ( F P k − 1 ∣ k − 1 ) F ⊤ + Q \mathbf{P}_{k|k-1} = (\mathbf{F} \mathbf{P}_{k-1|k-1}) \mathbf{F}^\top + \mathbf{Q} Pk∣k−1=(FPk−1∣k−1)F⊤+Q
= [ P x x + 2 P x v x + P v x v x P x y + P x v y + P y v x + P v x v y P x a + P x v a + P a v x + P v x v a P x h + P x v h + P h v x + P v x v h P x v x + P v x v x P x v y + P v x v y P x v a + P v x v a P x v h + P v x v h P x y + P y v x + P x v y + P v x v y P y y + 2 P y v y + P v y v y P y a + P y v a + P a v y + P v y v a P y h + P y v h + P h v y + P v y v h P y v x + P v x v y P y v y + P v y v y P y v a + P v y v a P y v h + P v y v h P x a + P a v x + P x v a + P v x v a P y a + P a v y + P y v a + P v y v a P a a + 2 P a v a + P v a v a P a h + P a v h + P h v a + P v a v h P a v x + P v x v a P a v y + P v y v a P a v a + P v a v a P a v h + P v a v h P x h + P h v x + P x v h + P v x v h P y h + P h v y + P y v h + P v y v h P a h + P h v a + P a v h + P v a v h P h h + 2 P h v h + P v h v h P h v x + P v x v h P h v y + P v y v h P h v a + P v a v h P h v h + P v h v h P x v x + P v x v x P y v x + P v x v y P a v x + P v x v a P h v x + P v x v h P v x v x P v x v y P v x v a P v x v h P x v y + P v x v y P y v y + P v y v y P a v y + P v y v a P h v y + P v y v h P v x v y P v y v y P v y v a P v y v h P x v a + P v x v a P y v a + P v y v a P a v a + P v a v a P h v a + P v a v h P v x v a P v y v a P v a v a P v a v h P x v h + P v x v h P y v h + P v y v h P a v h + P v a v h P h v h + P v h v h P v x v h P v y v h P v a v h P v h v h ] + [ σ x 2 0 0 0 0 0 0 0 0 σ y 2 0 0 0 0 0 0 0 0 σ a 2 0 0 0 0 0 0 0 0 σ h 2 0 0 0 0 0 0 0 0 σ v x 2 0 0 0 0 0 0 0 0 σ v y 2 0 0 0 0 0 0 0 0 σ v a 2 0 0 0 0 0 0 0 0 σ v h 2 ] = \begin{bmatrix} P_{xx} + 2P_{xvx} + P_{vxvx} & P_{xy} + P_{xvy} + P_{yvx} + P_{vxvy} & P_{xa} + P_{xva} + P_{avx} + P_{vxva} & P_{xh} + P_{xvh} + P_{hvx} + P_{vxvh} & P_{xvx} + P_{vxvx} & P_{xvy} + P_{vxvy} & P_{xva} + P_{vxva} & P_{xvh} + P_{vxvh} \\ P_{xy} + P_{yvx} + P_{xvy} + P_{vxvy} & P_{yy} + 2P_{yvy} + P_{vyvy} & P_{ya} + P_{yva} + P_{avy} + P_{vyva} & P_{yh} + P_{yvh} + P_{hvy} + P_{vyvh} & P_{yvx} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{yva} + P_{vyva} & P_{yvh} + P_{vyvh} \\ P_{xa} + P_{avx} + P_{xva} + P_{vxva} & P_{ya} + P_{avy} + P_{yva} + P_{vyva} & P_{aa} + 2P_{ava} + P_{vava} & P_{ah} + P_{avh} + P_{hva} + P_{vavh} & P_{avx} + P_{vxva} & P_{avy} + P_{vyva} & P_{ava} + P_{vava} & P_{avh} + P_{vavh} \\ P_{xh} + P_{hvx} + P_{xvh} + P_{vxvh} & P_{yh} + P_{hvy} + P_{yvh} + P_{vyvh} & P_{ah} + P_{hva} + P_{avh} + P_{vavh} & P_{hh} + 2P_{hvh} + P_{vhvh} & P_{hvx} + P_{vxvh} & P_{hvy} + P_{vyvh} & P_{hva} + P_{vavh} & P_{hvh} + P_{vhvh} \\ P_{xvx} + P_{vxvx} & P_{yvx} + P_{vxvy} & P_{avx} + P_{vxva} & P_{hvx} + P_{vxvh} & P_{vxvx} & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{avy} + P_{vyva} & P_{hvy} + P_{vyvh} & P_{vxvy} & P_{vyvy} & P_{vyva} & P_{vyvh} \\ P_{xva} + P_{vxva} & P_{yva} + P_{vyva} & P_{ava} + P_{vava} & P_{hva} + P_{vavh} & P_{vxva} & P_{vyva} & P_{vava} & P_{vavh} \\ P_{xvh} + P_{vxvh} & P_{yvh} + P_{vyvh} & P_{avh} + P_{vavh} & P_{hvh} + P_{vhvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} \end{bmatrix} + \begin{bmatrix} \sigma_{x}^2 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{y}^2 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{a}^2 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{h}^2 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{vx}^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{vy}^2 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{va}^2 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{vh}^2 \end{bmatrix} = Pxx+2Pxvx+PvxvxPxy+Pyvx+Pxvy+PvxvyPxa+Pavx+Pxva+PvxvaPxh+Phvx+Pxvh+PvxvhPxvx+PvxvxPxvy+PvxvyPxva+PvxvaPxvh+PvxvhPxy+Pxvy+Pyvx+PvxvyPyy+2Pyvy+PvyvyPya+Pavy+Pyva+PvyvaPyh+Phvy+Pyvh+PvyvhPyvx+PvxvyPyvy+PvyvyPyva+PvyvaPyvh+PvyvhPxa+Pxva+Pavx+PvxvaPya+Pyva+Pavy+PvyvaPaa+2Pava+PvavaPah+Phva+Pavh+PvavhPavx+PvxvaPavy+PvyvaPava+PvavaPavh+PvavhPxh+Pxvh+Phvx+PvxvhPyh+Pyvh+Phvy+PvyvhPah+Pavh+Phva+PvavhPhh+2Phvh+PvhvhPhvx+PvxvhPhvy+PvyvhPhva+PvavhPhvh+PvhvhPxvx+PvxvxPyvx+PvxvyPavx+PvxvaPhvx+PvxvhPvxvxPvxvyPvxvaPvxvhPxvy+PvxvyPyvy+PvyvyPavy+PvyvaPhvy+PvyvhPvxvyPvyvyPvyvaPvyvhPxva+PvxvaPyva+PvyvaPava+PvavaPhva+PvavhPvxvaPvyvaPvavaPvavhPxvh+PvxvhPyvh+PvyvhPavh+PvavhPhvh+PvhvhPvxvhPvyvhPvavhPvhvh + σx200000000σy200000000σa200000000σh200000000σvx200000000σvy200000000σva200000000σvh2
最终得到：
P k ∣ k − 1 = [ P x x + 2 P x v x + P v x v x + σ x 2 P x y + P x v y + P y v x + P v x v y P x a + P x v a + P a v x + P v x v a P x h + P x v h + P h v x + P v x v h P x v x + P v x v x P x v y + P v x v y P x v a + P v x v a P x v h + P v x v h P x y + P y v x + P x v y + P v x v y P y y + 2 P y v y + P v y v y + σ y 2 P y a + P y v a + P a v y + P v y v a P y h + P y v h + P h v y + P v y v h P y v x + P v x v y P y v y + P v y v y P y v a + P v y v a P y v h + P v y v h P x a + P a v x + P x v a + P v x v a P y a + P a v y + P y v a + P v y v a P a a + 2 P a v a + P v a v a + σ a 2 P a h + P a v h + P h v a + P v a v h P a v x + P v x v a P a v y + P v y v a P a v a + P v a v a P a v h + P v a v h P x h + P h v x + P x v h + P v x v h P y h + P h v y + P y v h + P v y v h P a h + P h v a + P a v h + P v a v h P h h + 2 P h v h + P v h v h + σ h 2 P h v x + P v x v h P h v y + P v y v h P h v a + P v a v h P h v h + P v h v h P x v x + P v x v x P y v x + P v x v y P a v x + P v x v a P h v x + P v x v h P v x v x + σ v x 2 P v x v y P v x v a P v x v h P x v y + P v x v y P y v y + P v y v y P a v y + P v y v a P h v y + P v y v h P v x v y P v y v y + σ v y 2 P v y v a P v y v h P x v a + P v x v a P y v a + P v y v a P a v a + P v a v a P h v a + P v a v h P v x v a P v y v a P v a v a + σ v a 2 P v a v h P x v h + P v x v h P y v h + P v y v h P a v h + P v a v h P h v h + P v h v h P v x v h P v y v h P v a v h P v h v h + σ v h 2 ] \mathbf{P}_{k|k-1} = \begin{bmatrix} P_{xx} + 2P_{xvx} + P_{vxvx} + \sigma_{x}^2 & P_{xy} + P_{xvy} + P_{yvx} + P_{vxvy} & P_{xa} + P_{xva} + P_{avx} + P_{vxva} & P_{xh} + P_{xvh} + P_{hvx} + P_{vxvh} & P_{xvx} + P_{vxvx} & P_{xvy} + P_{vxvy} & P_{xva} + P_{vxva} & P_{xvh} + P_{vxvh} \\ P_{xy} + P_{yvx} + P_{xvy} + P_{vxvy} & P_{yy} + 2P_{yvy} + P_{vyvy} + \sigma_{y}^2 & P_{ya} + P_{yva} + P_{avy} + P_{vyva} & P_{yh} + P_{yvh} + P_{hvy} + P_{vyvh} & P_{yvx} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{yva} + P_{vyva} & P_{yvh} + P_{vyvh} \\ P_{xa} + P_{avx} + P_{xva} + P_{vxva} & P_{ya} + P_{avy} + P_{yva} + P_{vyva} & P_{aa} + 2P_{ava} + P_{vava} + \sigma_{a}^2 & P_{ah} + P_{avh} + P_{hva} + P_{vavh} & P_{avx} + P_{vxva} & P_{avy} + P_{vyva} & P_{ava} + P_{vava} & P_{avh} + P_{vavh} \\ P_{xh} + P_{hvx} + P_{xvh} + P_{vxvh} & P_{yh} + P_{hvy} + P_{yvh} + P_{vyvh} & P_{ah} + P_{hva} + P_{avh} + P_{vavh} & P_{hh} + 2P_{hvh} + P_{vhvh} + \sigma_{h}^2 & P_{hvx} + P_{vxvh} & P_{hvy} + P_{vyvh} & P_{hva} + P_{vavh} & P_{hvh} + P_{vhvh} \\ P_{xvx} + P_{vxvx} & P_{yvx} + P_{vxvy} & P_{avx} + P_{vxva} & P_{hvx} + P_{vxvh} & P_{vxvx} + \sigma_{vx}^2 & P_{vxvy} & P_{vxva} & P_{vxvh} \\ P_{xvy} + P_{vxvy} & P_{yvy} + P_{vyvy} & P_{avy} + P_{vyva} & P_{hvy} + P_{vyvh} & P_{vxvy} & P_{vyvy} + \sigma_{vy}^2 & P_{vyva} & P_{vyvh} \\ P_{xva} + P_{vxva} & P_{yva} + P_{vyva} & P_{ava} + P_{vava} & P_{hva} + P_{vavh} & P_{vxva} & P_{vyva} & P_{vava} + \sigma_{va}^2 & P_{vavh} \\ P_{xvh} + P_{vxvh} & P_{yvh} + P_{vyvh} & P_{avh} + P_{vavh} & P_{hvh} + P_{vhvh} & P_{vxvh} & P_{vyvh} & P_{vavh} & P_{vhvh} + \sigma_{vh}^2 \end{bmatrix} Pk∣k−1= Pxx+2Pxvx+Pvxvx+σx2Pxy+Pyvx+Pxvy+PvxvyPxa+Pavx+Pxva+PvxvaPxh+Phvx+Pxvh+PvxvhPxvx+PvxvxPxvy+PvxvyPxva+PvxvaPxvh+PvxvhPxy+Pxvy+Pyvx+PvxvyPyy+2Pyvy+Pvyvy+σy2Pya+Pavy+Pyva+PvyvaPyh+Phvy+Pyvh+PvyvhPyvx+PvxvyPyvy+PvyvyPyva+PvyvaPyvh+PvyvhPxa+Pxva+Pavx+PvxvaPya+Pyva+Pavy+PvyvaPaa+2Pava+Pvava+σa2Pah+Phva+Pavh+PvavhPavx+PvxvaPavy+PvyvaPava+PvavaPavh+PvavhPxh+Pxvh+Phvx+PvxvhPyh+Pyvh+Phvy+PvyvhPah+Pavh+Phva+PvavhPhh+2Phvh+Pvhvh+σh2Phvx+PvxvhPhvy+PvyvhPhva+PvavhPhvh+PvhvhPxvx+PvxvxPyvx+PvxvyPavx+PvxvaPhvx+PvxvhPvxvx+σvx2PvxvyPvxvaPvxvhPxvy+PvxvyPyvy+PvyvyPavy+PvyvaPhvy+PvyvhPvxvyPvyvy+σvy2PvyvaPvyvhPxva+PvxvaPyva+PvyvaPava+PvavaPhva+PvavhPvxvaPvyvaPvava+σva2PvavhPxvh+PvxvhPyvh+PvyvhPavh+PvavhPhvh+PvhvhPvxvhPvyvhPvavhPvhvh+σvh2
这篇关于DeepSORT（目标跟踪算法）中卡尔曼滤波器中的预测的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
