DeepSORT（目标跟踪算法）中的初始化卡尔曼滤波器的状态向量和协方差矩阵

DeepSORT（目标跟踪算法）中的初始化卡尔曼滤波器的状态向量和协方差矩阵

flyfish

如果看了下面遇到了状态转移矩阵，可以先看 DeepSORT（目标跟踪算法）中的卡尔曼滤波 - 看了就会的状态转移矩阵 ，这里做了非常详细的描述

import numpy as np

np.set_printoptions(suppress=True)

class KalmanFilter(object):
    def __init__(self):
        ndim, dt = 4, 1.0

        # 创建卡尔曼滤波模型矩阵
        self._motion_mat = np.eye(2 * ndim)
        for i in range(ndim):
            self._motion_mat[i, ndim + i] = dt
        #运动矩阵（F）将线性关系表达为矩阵形式    
        print("__init__ _motion_mat:",self._motion_mat)    
        
        
        #更新矩阵（H）用于将观测结果转换为状态向量。在这个例子中，观测结果只包含位置和大小（不包括速度），所以它是一个4x8的矩阵：
        self._update_mat = np.eye(ndim, 2 * ndim)
        print("__init__ _update_mat:",self._update_mat)  

    
        
        # _std_weight_position = 1. / 20 表示位置的不确定性权重，值为0.05。
        # _std_weight_velocity = 1. / 160 表示速度的不确定性权重，值为0.00625。
        #位置的不确定性较大，速度的不确定性较小，这使得滤波器对位置变化更加敏感，而对速度变化更加稳定。
        self._std_weight_position = 1. / 20
        self._std_weight_velocity = 1. / 160

    def initiate(self, measurement):

        #mean_pos：代表观测到的位置向量 measurement。
        # 假设 measurement 包含物体的检测信息（如中心点的坐标和尺寸），通常为一个长度为4的向量 [x, y, a, h]，其中 x 和 y 是位置坐标，a 是纵横比（宽/高），h 是高度。
        mean_pos = measurement
        print("initiate mean_pos:",mean_pos)
        
        
        #mean_vel：代表速度向量的初始均值。这里初始化为与 mean_pos 相同长度的零向量，因为在初始化时，通常没有速度信息
        mean_vel = np.zeros_like(mean_pos)
        print("initiate mean_vel:",mean_vel)
        
        #方便的垂直拼接数组
        mean = np.r_[mean_pos, mean_vel]
        print("initiate mean:",mean)

        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]
        ]
        print("initiate std:",std)
        print("initiate np.square(std):",np.square(std))
        # np.square(std)计算标准差的平方，即方差。
        # np.diag(np.square(std)) 构造一个对角矩阵，主对角线上是方差值，其它位置为零。这形成了初始协方差矩阵，表示系统状态的不确定性。
        
        #covariance矩阵表示初始状态的不确定性。
        covariance = np.diag(np.square(std))
        return mean, covariance


# 示例检测到的物体位置和尺寸
measurement = np.array([10, 5, 1.2, 4])

# 创建 KalmanFilter 实例
kf = KalmanFilter()

# 调用 initiate 方法
mean, covariance = kf.initiate(measurement)

# 输出结果
print("Mean:")
print(mean)
print("Covariance:")
print(covariance)

上面代码加了注释，也可以先看 DeepSORT（目标跟踪算法）中的卡尔曼滤波 - 看了就会的状态转移矩阵 更详细。
结果

__init__ _motion_mat: [[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.]]
__init__ _update_mat: [[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.]]
initiate mean_pos: [10.   5.   1.2  4. ]
initiate mean_vel: [0. 0. 0. 0.]
initiate mean: [10.   5.   1.2  4.   0.   0.   0.   0. ]
initiate std: [0.4, 0.4, 0.01, 0.4, 0.25, 0.25, 1e-05, 0.25]
initiate np.square(std): [0.16   0.16   0.0001 0.16   0.0625 0.0625 0.     0.0625]
Mean:
[10.   5.   1.2  4.   0.   0.   0.   0. ]
Covariance:
[[0.16   0.     0.     0.     0.     0.     0.     0.    ]
 [0.     0.16   0.     0.     0.     0.     0.     0.    ]
 [0.     0.     0.0001 0.     0.     0.     0.     0.    ]
 [0.     0.     0.     0.16   0.     0.     0.     0.    ]
 [0.     0.     0.     0.     0.0625 0.     0.     0.    ]
 [0.     0.     0.     0.     0.     0.0625 0.     0.    ]
 [0.     0.     0.     0.     0.     0.     0.     0.    ]
 [0.     0.     0.     0.     0.     0.     0.     0.0625]]

代码中的 self._motion_mat（运动矩阵）实际上就是状态转移矩阵（State Transition Matrix），通常表示为 $\mathbf{F}$。在卡尔曼滤波器的术语中，运动矩阵和状态转移矩阵指的是同一个概念，即描述系统状态在时间上的演变关系。

• 运动矩阵（Motion Matrix）：这个名称强调了该矩阵描述的是系统状态如何随时间变化，即系统的运动学特性。
• 状态转移矩阵（State Transition Matrix）：这个名称更通用，强调了该矩阵在卡尔曼滤波中的作用，即将当前状态转移到下一时刻的状态。
  两者虽然名称不同，但在具体应用中它们的作用和定义是相同的。在不同的文献或代码实现中，有不同的名称来强调其特定的用途或背景，但本质上它们是相同的。

对于状态向量 $\mathbf{z}$：
$$\mathbf{z}=[x,y,a,h,vx,vy,va,vh{]}^T$$
这里：

• $x$ 和 $y$ 是位置坐标，
• $a$ 是纵横比（宽度/高度），
• $h$ 是高度，
• $vx$ 和 $vy$ 是位置的速度，
• $va$ 是纵横比的变化率，
• $vh$ 是高度的变化率。
  std 是一个包含标准差的列表，用于初始化协方差矩阵。每个标准差对应于状态向量中不同元素的不确定性。具体的标准差取值和倍率因素如下：

不确定性采用了measurement[3]，它是当前测量值中的高度

2 * self._std_weight_position * measurement[3]：代表位置 x 的标准差，乘以位置权重系数和高度（或某个相关度量）。self._std_weight_position 是位置的不确定性权重。
2 * self._std_weight_position * measurement[3]：代表位置 y 的标准差。
1e-2：代表纵横比 a 的标准差，一个固定的小值。
2 * self._std_weight_position * measurement[3]：代表高度 h 的标准差。
10 * self._std_weight_velocity * measurement[3]：代表速度 vx 的标准差，乘以速度权重系数和高度。self._std_weight_velocity 是速度的不确定性权重。
10 * self._std_weight_velocity * measurement[3]：代表速度 vy 的标准差。
1e-5：代表纵横比变化率 va 的标准差，一个固定的很小的值。
10 * self._std_weight_velocity * measurement[3]：代表高度变化率 vh 的标准差。

倍率因素（2* 和 10*）
倍率因素是根据经验或具体应用调整的，用于控制不确定性的初始值：

2 *：位置的不确定性权重。位置的不确定性通常根据物体检测框的大小（如高度）来确定。
10 *：速度的不确定性权重。速度的不确定性通常比位置的不确定性大，因此乘以一个较大的系数，以反映速度估计的高不确定性。

协方差矩阵
也就是上面代码中的Covariance
描述状态估计的不确定性，是一个对称矩阵。在初始化时，它通常是对角矩阵，对角线元素表示各个状态变量的初始方差。在预测和更新步骤中，协方差矩阵会不断调整以反映新的不确定性。协方差矩阵在卡尔曼滤波器中用于描述状态估计的不确定性。具体来说，它表示状态向量中每个元素的方差（不确定性）以及这些元素之间的协方差。协方差矩阵是对称的，并且在滤波器的预测和更新步骤中不断更新。