DeepSORT（目标跟踪算法）中卡尔曼滤波器中的更新

flyfish

说协方差先说期望

在协方差的定义中，符号 $\mathbb{E}$ 表示期望值（Expectation）。期望值是随机变量的平均值或均值，表示在大量试验中随机变量的平均结果。

期望值的详细解释

期望值 $\mathbb{E}[X]$ 是指随机变量 $X$ 在概率意义上的平均值。它可以看作是随机变量在无限次重复试验中的平均值。

离散型随机变量的期望值

对于离散型随机变量 $X$ 取值 $x_1, x_2, \ldots, x_n$ ，并且对应的概率为 $p_1, p_2, \ldots, p_n$ ，其期望值定义为：

$\mathbb{E}[X] = \sum_{i=1}^{n} x_i \cdot p_i$

即每个取值 $x_i$ 乘以其对应的概率 $p_i$ ，然后将这些乘积求和。

连续型随机变量的期望值

对于连续型随机变量 $X$ 具有概率密度函数 $f (x)$ ，其期望值定义为：

$\mathbb{E}[X] = \int_{-\infty}^{\infty} x \cdot f(x) \, dx$

即对 $x$ 和其概率密度函数 $f (x)$ 的乘积进行积分。

在协方差定义中的期望值

在协方差的定义中，期望值 $\mathbb{E}$ 用于计算随机变量的均值，以及偏离均值的乘积的均值：

$\text{Cov}(X, Y) = \mathbb{E}[(X - \mathbb{E}[X])(Y - \mathbb{E}[Y])]$

具体步骤如下：

计算均值（期望值）：

计算 $X$ 的均值： $\mathbb{E}[X]$
计算 $Y$ 的均值： $\mathbb{E}[Y]$

中心化变量：

对 $X$ 做中心化： $\mathbb{E}[X]$
对 $Y$ 做中心化： $\mathbb{E}[Y]$

计算中心化变量的乘积的期望值：

计算 $\mathbb{E}[X])(Y - \mathbb{E}[Y])$ 的期望值，即平均值。
这样得到的结果就是 $X$ 和 $Y$ 的协方差，表示它们的共同变化程度。

举例说明期望值

假设我们有一个离散型随机变量 $X$ ，其取值和概率如下：

$X$	1	2	3	4	5
$P (X)$	0.1	0.2	0.3	0.2	0.2

其期望值 $\mathbb{E}[X]$ 计算如下：

$\mathbb{E}[X] = 1 \cdot 0.1 + 2 \cdot 0.2 + 3 \cdot 0.3 + 4 \cdot 0.2 + 5 \cdot 0.2 = 0.1 + 0.4 + 0.9 + 0.8 + 1 = 3.2$

这表示，如果我们重复进行很多次试验，随机变量 $X$ 的平均值会趋近于 3.2。

协方差的定义

协方差是统计学中用于度量两个随机变量共同变化程度的指标。给定两个随机变量 $X$ 和 $Y$ ，它们的协方差定义如下：

$\text{Cov}(X, Y) = \mathbb{E}[(X - \mathbb{E}[X])(Y - \mathbb{E}[Y])]$

让我们逐步解释这个公式：

期望值 ( $\mathbb{E}$ )：

期望值是随机变量的平均值或均值，表示在大量试验中随机变量的平均结果。
$\mathbb{E}[X]$ 表示 $X$ 的期望值（均值）， $\mathbb{E}[Y]$ 表示 $Y$ 的期望值。

中心化：

$\mathbb{E}[X]$ 表示 $X$ 偏离其均值的程度，称为 $X$ 的中心化。
$\mathbb{E}[Y]$ 表示 $Y$ 偏离其均值的程度，称为 $Y$ 的中心化。

乘积：

$\mathbb{E}[X])(Y - \mathbb{E}[Y])$ 表示 $X$ 和 $Y$ 各自偏离均值的乘积。

期望值：

$\mathbb{E}[(X - \mathbb{E}[X])(Y - \mathbb{E}[Y])]$ 表示这些偏离乘积的平均值，这就是协方差。

协方差的含义

协方差度量了两个变量如何一起变化：

正协方差：如果协方差是正的，表示当 $X$ 增大时 $Y$ 也倾向于增大，反之亦然。这意味着 $X$ 和 $Y$ 之间有正相关关系。
负协方差：如果协方差是负的，表示当 $X$ 增大时 $Y$ 倾向于减小，反之亦然。这意味着 $X$ 和 $Y$ 之间有负相关关系。
零协方差：如果协方差是零，表示 $X$ 和 $Y$ 之间没有线性关系。

简单例子

假设我们有两个变量 $X$ 和 $Y$ ，并且有几组观测值：

$X$	$Y$
1	2
2	3
3	6
4	8
5	10

计算均值：

$\mathbb{E}[X] = \frac{1+2+3+4+5}{5} = 3$
$\mathbb{E}[Y] = \frac{2+3+6+8+10}{5} = 5.8$

中心化并计算偏离乘积：

对于每个观测值，计算 $\mathbb{E}[X])(Y - \mathbb{E}[Y])$ ：
第一组： $\times 3.8 = 7.6$
第二组： $\times 2.8 = 2.8$
第三组： $\times 0.2 = 0$
第四组： $\times 2.2 = 2.2$
第五组： $\times 4.2 = 8.4$

计算协方差：

取这些乘积的平均值：
$\text{Cov}(X, Y) = \frac{7.6 + 2.8 + 0 + 2.2 + 8.4}{5} = \frac{21}{5} = 4.2$

综述

协方差通过衡量两个变量共同偏离均值的方式来表示它们之间的关系。正协方差表示正相关，负协方差表示负相关，零协方差表示没有线性关系。通过理解这些，可以更好地理解数据中不同变量之间的关系。

# 定义随机变量和对应的概率
X_values = [1, 2, 3, 4, 5]
P_X = [0.1, 0.2, 0.3, 0.2, 0.2]

# 计算期望值
expected_value = sum(x * p for x, p in zip(X_values, P_X))
print("期望值 (均值) =", expected_value)

期望值 (均值) = 3.2

代码展示

import numpy as np
import pandas as pd
import seaborn as sns
import matplotlib.pyplot as plt

# 生成随机数据
np.random.seed(42)
data = np.random.randn(100, 5)  # 生成100个样本，每个样本有5个特征
columns = ['Feature1', 'Feature2', 'Feature3', 'Feature4', 'Feature5']
df = pd.DataFrame(data, columns=columns)

# 计算协方差矩阵
cov_matrix = df.cov()

# 使用热图可视化协方差矩阵
plt.figure(figsize=(10, 8))
sns.heatmap(cov_matrix, annot=True, fmt='.2f', cmap='coolwarm', square=True, cbar_kws={"shrink": .8})
plt.title('Covariance Matrix Heatmap')
plt.show()

在这里插入图片描述
通过热图等可视化方法，可以直观地观察协方差矩阵，理解各变量间的相关性及其不确定性。
卡尔曼滤波器主要通过两个噪声协方差矩阵来表示系统中的不确定性：
过程噪声协方差矩阵（ $Q$ ）和测量噪声协方差矩阵（ $R$ ）。

1. 过程噪声协方差矩阵（ $Q$ ）

过程噪声协方差矩阵 $Q$ 表示系统模型中的不确定性。具体来说：

过程噪声：过程噪声（也称为模型噪声或系统噪声）是指系统的状态在每一步更新时所包含的随机误差。这种噪声可能来源于系统建模的不完善、外界扰动或者其他不可预测的变化。
协方差矩阵 $Q$ ：矩阵 $Q$ 的每个元素表示系统不同状态变量之间噪声的协方差。对角线元素表示各个状态变量自身的不确定性，非对角线元素表示不同状态变量之间的相关性。

2. 测量噪声协方差矩阵（ $R$ ）

测量噪声协方差矩阵 $R$ 表示传感器测量中的不确定性。具体来说：

测量噪声：测量噪声是指传感器在获取系统状态测量值时所包含的随机误差。它可能来源于传感器的精度限制、外部环境的干扰等。
协方差矩阵 $R$ ：矩阵 $R$ 的每个元素表示不同测量值之间噪声的协方差。对角线元素表示各个测量值自身的不确定性，非对角线元素表示不同测量值之间的相关性。

3. 不确定性的表达

通过 $Q$ 和 $R$ 矩阵，卡尔曼滤波器在以下方面表达和处理不确定性：

状态预测：在预测步骤中，卡尔曼滤波器使用过程噪声协方差矩阵 $Q$ 来估计由于过程噪声引入的不确定性，从而更新预测状态的协方差。
测量更新：在更新步骤中，卡尔曼滤波器使用测量噪声协方差矩阵 $R$ 来衡量测量噪声的不确定性，从而更新状态估计和协方差矩阵。

卡尔曼增益 $K$ 的计算公式中确实包含了过程噪声协方差矩阵 $Q$ 和测量噪声协方差矩阵 $R$

卡尔曼增益 $K$ 的计算公式

卡尔曼增益 $K$ 是在更新步骤中计算的，公式如下：

$K = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1}$

其中：

$P_k^-$ 是预测误差协方差矩阵。
$H_k$ 是观测矩阵。
$R$ 是测量噪声协方差矩阵。

过程噪声协方差矩阵 $Q$ 和测量噪声协方差矩阵 $R$

这两个矩阵在卡尔曼滤波器的预测和更新步骤中起到关键作用：

预测步骤：（DeepSORT（目标跟踪算法）中卡尔曼滤波器中的预测）

状态预测：
$\hat{x}_k^- = A \hat{x}_{k-1} + B u_{k-1}$
误差协方差预测：
$P_k^- = A P_{k-1} A^T + Q$ 在这里，过程噪声协方差矩阵 $Q$ 用于更新预测误差协方差矩阵 $P_k^-$ 。它反映了系统内部的随机变化或不确定性。

更新步骤：

计算卡尔曼增益：
$K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1}$
状态更新：
$\hat{x}_k = \hat{x}_k^- + K_k (z_k - H_k \hat{x}_k^-)$
误差协方差更新：
$P_k = (I - K_k H_k) P_k^-$ 在这里，测量噪声协方差矩阵 $R$ 直接出现在卡尔曼增益的计算公式中。它反映了观测数据中的噪声和不确定性。

从卡尔曼增益的计算公式中，我们可以看出：

预测误差协方差矩阵 $P_k^-$ 包含了过程噪声协方差矩阵 $Q$ ，因为 $P_k^-$ 是通过前一个时间步的误差协方差矩阵 $P_{k-1}$ 和过程噪声协方差矩阵 $Q$ 进行更新的。具体地， $P_k^- = A P_{k-1} A^T + Q$ 。
测量噪声协方差矩阵 $R$ 直接出现在卡尔曼增益的分母部分（即 $H_k P_k^- H_k^T + R$ 中），影响了卡尔曼增益的大小。
过程噪声协方差矩阵 $Q$ 影响预测误差协方差矩阵 $P_k^-$ ，从而间接影响卡尔曼增益 $K$ 。
测量噪声协方差矩阵 $R$ 直接出现在卡尔曼增益的计算公式中，影响了卡尔曼增益 $K$ 的值。
卡尔曼增益 $K$ 的计算公式通过组合预测误差协方差矩阵 $P_k^-$ 和测量噪声协方差矩阵 $R$ 来确定当前测量和预测在更新状态估计时的权重，从而实现对系统状态的最优估计。

状态估计

状态估计是指系统的当前状态向量的估计值。这通常是一个包含系统所有状态变量的向量。比如，假设我们在跟踪一个运动物体的状态，其状态向量可能包含位置和速度：

$\hat{x}_k = \begin{bmatrix} \text{position}_x \\ \text{position}_y \\ \text{velocity}_x \\ \text{velocity}_y \end{bmatrix}$

在卡尔曼滤波器中，状态估计分为预测估计和更新估计：

预测状态估计：通过系统动态模型从先前的状态估计得到当前时间步的预测状态。
$\hat{x}_k^- = A \hat{x}_{k-1} + B u_{k-1}$
更新状态估计：结合预测估计和当前的测量值来更新状态估计。
$\hat{x}_k = \hat{x}_k^- + K_k (z_k - H_k \hat{x}_k^-)$

误差协方差矩阵

协方差矩阵用于描述状态估计的不确定性，它表示状态估计中的误差分布情况。误差协方差矩阵的对角元素表示各个状态变量估计的方差，非对角元素表示状态变量之间的协方差。

在卡尔曼滤波器中，协方差矩阵同样分为预测协方差矩阵和更新协方差矩阵：

预测误差协方差矩阵：通过系统动态模型从先前的误差协方差矩阵得到当前时间步的预测误差协方差矩阵。
$P_k^- = A P_{k-1} A^T + Q$
这里， $P_k^-$ 是预测误差协方差矩阵， $Q$ 是过程噪声协方差矩阵。
更新误差协方差矩阵：结合预测误差协方差矩阵和测量噪声协方差矩阵更新误差协方差矩阵。
$P_k = (I - K_k H_k) P_k^-$

两者的比较

状态估计：是一个向量，表示系统的状态变量的估计值。
误差协方差矩阵：是一个矩阵，表示状态估计的误差及其不确定性。

project 方法

该方法将状态分布投影到测量空间，这是更新步骤的一部分，用于计算测量预测和测量预测的协方差。

def project(self, mean, covariance):
    """将状态分布投影到测量空间。

    参数
    ----------
    mean : ndarray
        状态的均值向量（8维数组）。
    covariance : ndarray
        状态的协方差矩阵（8x8维）。

    返回
    -------
    (ndarray, ndarray)
        返回给定状态估计的投影均值和协方差矩阵。

    """
    # 计算测量噪声标准差
    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

输入：状态均值向量 mean 和状态协方差矩阵 covariance。
过程：计算测量噪声协方差 innovation_cov。使用 self._update_mat 将状态均值投影到测量空间。使用 self._update_mat 计算投影后的协方差矩阵。
输出：投影后的均值和协方差矩阵。
这对应于卡尔曼滤波的预测测量和预测测量的协方差：

$\hat{z}_k = H_k \hat{x}_k^-$
$S_k = H_k P_k^- H_k^T + R$

update 方法

该方法运行卡尔曼滤波器的更新步骤，使用新测量值来更新状态估计和协方差矩阵。

def update(self, mean, covariance, measurement):
    """运行卡尔曼滤波器的校正步骤。

    参数
    ----------
    mean : ndarray
        预测状态的均值向量（8维）。
    covariance : ndarray
        状态的协方差矩阵（8x8维）。
    measurement : ndarray
        4维测量向量（x, y, a, h），其中（x, y）是边界框的中心位置，a是长宽比，h是高度。

    返回
    -------
    (ndarray, ndarray)
        返回校正后的状态分布。

    """
    # 计算投影的均值和协方差矩阵
    projected_mean, projected_cov = self.project(mean, covariance)

    # 计算卡尔曼增益
    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

输入：预测状态均值向量 mean、状态协方差矩阵 covariance 和测量值 measurement。
过程：调用 project 方法计算投影后的均值和协方差矩阵。使用 scipy.linalg.cho_factor 和 scipy.linalg.cho_solve 计算卡尔曼增益 kalman_gain。计算测量创新 innovation，即测量值与预测测量值的差异。更新状态均值 new_mean 和协方差矩阵 new_covariance。
输出：更新后的状态均值和协方差矩阵。
这对应于卡尔曼滤波的更新步骤：

$K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^{-1}$
$\hat{x}_k = \hat{x}_k^- + K_k (z_k - \hat{z}_k)$
$P_k = (I - K_k H_k) P_k^-$