DeepSORT（目标跟踪算法） 卡尔曼滤波 状态向量是如何映射到观测向量（测量向量）的即观测矩阵的构建方式

DeepSORT（目标跟踪算法） 卡尔曼滤波 状态向量是如何映射到观测向量（测量向量）的即观测矩阵的构建方式

flyfish
测量向量和观测变量在卡尔曼滤波的上下文中通常是同一个意思。它们都指的是从系统中直接获得的数据，这些数据用于更新系统的状态估计。可以是从传感器或测量设备直接获得的数据。这些数据反映了系统在某一时刻的状态或者实际观测到的值，但通常带有噪声。

状态向量映射到观测向量的过程通过观测矩阵 $\mathbf{H}$ 实现。观测矩阵 $\mathbf{H}$ 描述了系统状态如何映射到观测值。下面通过一个具体的例子来详细说明这一过程。

构造观测矩阵 $\mathbf{H}$ 的步骤包括：

1. 定义状态变量：明确系统的状态变量。
2. 定义观测变量：明确系统的观测变量。
3. 写出观测方程：根据观测变量和状态变量之间的关系写出观测方程。
4. 构造观测矩阵：根据观测方程提取观测矩阵 $\mathbf{H}$。

例子：一维位置和速度的观测

假设我们有一个物体在一维直线上运动，我们希望估计其位置和速度，并且我们可以直接观测到位置，但不能直接观测到速度。

定义状态变量

状态向量定义为：
${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ v_k\end{array}\right]$
其中，$x_k$ 是位置，$v_k$ 是速度。

观测模型

我们可以直接测量位置 $x_k$，但不能直接测量速度 $v_k$。因此，观测向量定义为：
${\mathbf{z}}_k=\left[\begin{array}{c}{z}_k\end{array}\right]$
其中，$z_k$ 是我们观测到的位置。

观测方程

观测方程描述了观测向量如何由状态向量生成。在这个例子中，观测向量只包含位置，因此观测矩阵 $\mathbf{H}$ 为：
${\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$
其中，${\mathbf{v}}_k$ 是观测噪声。

对于这个例子，观测矩阵 $\mathbf{H}$ 是：
$\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$

这样，观测方程可以写成：
$z_k=1\cdot x_k+0\cdot v_k+v_k$

即：
$z_k=x_k+v_k$

构造观测矩阵 $\mathbf{H}$

通过上面的分析，我们得到了观测矩阵 $\mathbf{H}$：
$\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$

另一个例子：二维位置和速度的观测

假设我们有一个物体在二维平面上运动，我们希望估计其二维位置和速度，并且我们可以直接观测到位置，但不能直接观测到速度。

定义状态变量

状态向量定义为：
${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\\ v_{y,k}\end{array}\right]$
其中，$x_k$ 和 $y_k$ 是位置，$v_{x,k}$ 和 $v_{y,k}$ 是速度。

观测模型

我们可以直接测量位置 $x_k$ 和 $y_k$，但不能直接测量速度 $v_{x,k}$ 和 $v_{y,k}$。因此，观测向量定义为：
${\mathbf{z}}_k=\left[\begin{array}{c}{z}_{x,k}\\ z_{y,k}\end{array}\right]$
其中，$z_{x,k}$ 和 $z_{y,k}$ 是我们观测到的位置。

观测方程

观测方程描述了观测向量如何由状态向量生成。在这个例子中，观测向量只包含位置，因此观测矩阵 $\mathbf{H}$ 为：
${\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$
其中，${\mathbf{v}}_k$ 是观测噪声。

对于这个例子，观测矩阵 $\mathbf{H}$ 是：
$\mathbf{H}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$

这样，观测方程可以写成：
$\left[\begin{array}{c}{z}_{x,k}\\ z_{y,k}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_k\\ y_k\\ v_{x,k}\\ v_{y,k}\end{array}\right]+\left[\begin{array}{c}{v}_{x,k}\\ v_{y,k}\end{array}\right]$

即：
$\left[\begin{array}{c}{z}_{x,k}\\ z_{y,k}\end{array}\right]=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right]+\left[\begin{array}{c}{v}_{x,k}\\ v_{y,k}\end{array}\right]$

构造观测矩阵 $\mathbf{H}$

通过上面的分析，我们得到了观测矩阵 $\mathbf{H}$：
$\mathbf{H}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$

测量向量（Measurement Vector）

测量向量包含实际观测或测量得到的数据。它通常是状态向量的一部分或线性变换。

• 记作 ${\mathbf{z}}_k$，反映了系统在时间 $k$ 的观测数据。

观测矩阵（Observation Matrix）

观测矩阵将状态向量映射到测量向量，表示从状态向量到测量向量的关系。它定义了哪些状态变量是可观测的以及如何被观测。

• 记作 ${\mathbf{H}}_k$，用于从状态向量中提取测量向量：
  ${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$

关系与应用

• 测量向量与观测矩阵：观测矩阵 ${\mathbf{H}}_k$ 描述了如何从状态向量 ${\mathbf{x}}_k$ 中提取测量向量 ${\mathbf{z}}_k$。例如，如果我们只能测量位置而不能直接测量速度，那么观测矩阵可能是：${\mathbf{H}}_k=\left[\begin{array}{cc}1& 0\end{array}\right]$

例子

假设我们要跟踪一个在平面上运动的物体，其状态包括位置和速度：

• 状态向量 ${\mathbf{x}}_k$:${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ y_k\\ {\dot{x}}_k\\ {\dot{y}}_k\end{array}\right]$这里 $x_k$ 和 $y_k$ 是位置，${\dot{x}}_k$ 和 ${\dot{y}}_k$ 是速度。
• 状态转移矩阵 ${\mathbf{A}}_k$:${\mathbf{A}}_k=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$这表示位置随时间步长 $\Delta t$ 变化。
• 测量向量 ${\mathbf{z}}_k$:${\mathbf{z}}_k=\left[\begin{array}{c}{z}_{x_k}\\ z_{y_k}\end{array}\right]$这里 $z_{x_k}$ 和 $z_{y_k}$ 是测量得到的位置。
• 观测矩阵 ${\mathbf{H}}_k$:${\mathbf{H}}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$这表示我们只测量位置，而速度不可测。

在卡尔曼滤波中，预测步骤利用状态转移矩阵和控制输入预测系统的下一个状态。具体步骤如下：

测量向量通过观测矩阵可以得到预测测量值。这一过程是将状态向量映射到测量空间的关键步骤，用于比较实际测量值和预测测量值，从而更新状态估计。观测矩阵和测量残差一起在卡尔曼滤波器中发挥作用，使得状态估计更加准确和可靠。

观测矩阵的作用

观测矩阵（Observation Matrix）描述了状态向量与测量向量之间的关系。它将状态向量映射到测量空间，使得可以从状态向量中提取出测量向量。

测量向量与预测测量值

假设系统的状态向量为 ${\mathbf{x}}_k$，测量向量为 ${\mathbf{z}}_k$，观测矩阵为 ${\mathbf{H}}_k$。观测矩阵将状态向量映射到测量空间，得到预测测量值（或估计测量值）${\hat{\mathbf{z}}}_k$：
$${\hat{\mathbf{z}}}_k={\mathbf{H}}_k{\mathbf{x}}_k$$

具体步骤

1. 预测步骤：利用状态转移矩阵和控制输入预测下一时刻的状态向量 ${\hat{\mathbf{x}}}_{k|k-1}$。
2. 计算预测测量值：利用观测矩阵 ${\mathbf{H}}_k$ 将预测状态向量 ${\hat{\mathbf{x}}}_{k|k-1}$ 转换为预测测量值 ${\hat{\mathbf{z}}}_k$：
   ${\hat{\mathbf{z}}}_k={\mathbf{H}}_k{\hat{\mathbf{x}}}_{k|k-1}$
3. 更新步骤：比较预测测量值 ${\hat{\mathbf{z}}}_k$ 和实际测量值 ${\mathbf{z}}_k$，计算测量残差 ${\mathbf{y}}_k$，并用它来更新状态向量和误差协方差矩阵。

例子

假设我们跟踪一个物体，其状态向量包括位置和速度：
$${\mathbf{x}}_k=\left[\begin{array}{c}{x}_k\\ y_k\\ {\dot{x}}_k\\ {\dot{y}}_k\end{array}\right]$$
假设我们只能测量位置，而不能直接测量速度，观测矩阵可以表示为：
$${\mathbf{H}}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$
假设在时间步长 $k$ 的预测状态向量为：
$${\hat{\mathbf{x}}}_{k|k-1}=\left[\begin{array}{c}10\\ 15\\ 1\\ -1\end{array}\right]$$
观测矩阵将状态向量映射到测量空间，得到预测测量值：
$${\hat{\mathbf{z}}}_k={\mathbf{H}}_k{\hat{\mathbf{x}}}_{k|k-1}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}10\\ 15\\ 1\\ -1\end{array}\right]=\left[\begin{array}{c}10\\ 15\end{array}\right]$$

测量残差和更新

实际测量值可能为：
$${\mathbf{z}}_k=\left[\begin{array}{c}11\\ 14\end{array}\right]$$
测量残差（或创新）为：
$${\mathbf{y}}_k={\mathbf{z}}_k-{\hat{\mathbf{z}}}_k=\left[\begin{array}{c}11\\ 14\end{array}\right]-\left[\begin{array}{c}10\\ 15\end{array}\right]=\left[\begin{array}{c}1\\ -1\end{array}\right]$$
测量残差用于更新预测状态，使其更接近实际测量值。更新后的状态向量和误差协方差矩阵通过卡尔曼增益 ${\mathbf{K}}_k$ 进行修正：
$${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k{\mathbf{y}}_k$$
$${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k|k-1}$$