DeepSORT（目标跟踪算法） 卡尔曼滤波 状态空间模型的理解

DeepSORT（目标跟踪算法） 卡尔曼滤波 状态空间模型的理解

flyfish

卡尔曼滤波器是一种用于估计动态系统状态的递归算法，它基于状态空间模型进行工作。状态空间模型由两个主要方程组成：状态方程和观测方程。

状态方程描述系统的状态如何随时间演变，而观测方程描述如何通过测量值观察到系统的状态。

状态空间模型可以表示为：

1. 状态方程 ：
   x k = F x k − 1 + B u k + w k \mathbf{x}{k} = \mathbf{F} \mathbf{x}{k-1} + \mathbf{B} \mathbf{u}{k} + \mathbf{w}{k} xk=Fxk−1+Buk+wk
2. 观测方程 ：
   z k = H x k + v k \mathbf{z}{k} = \mathbf{H} \mathbf{x}{k} + \mathbf{v}_{k} zk=Hxk+vk
   其中：

• x k \mathbf{x}_{k} xk 是时刻 k k k 的状态向量。
• F \mathbf{F} F 是状态转移矩阵，描述状态如何从 k − 1 k-1 k−1 时刻转移到 k k k 时刻。
• B \mathbf{B} B 是控制输入矩阵，描述控制向量 u k \mathbf{u}_{k} uk 对状态的影响。
• w k \mathbf{w}_{k} wk 是过程噪声，假定为零均值的高斯白噪声。
• z k \mathbf{z}_{k} zk 是时刻 k k k 的观测向量。
• H \mathbf{H} H 是观测矩阵，描述状态向量如何映射到观测向量。
• v k \mathbf{v}_{k} vk 是观测噪声，假定为零均值的高斯白噪声。
  为了说明卡尔曼滤波中的状态空间模型和矩阵乘法之间的关系，下面举两个具体的例子：

例子 1：一维位置和速度估计

假设我们有一个在直线上运动的物体，我们希望估计它的位置信息和速度信息。

状态空间模型

• 状态向量： x k = [ x k v k ] \mathbf{x}{k} = \begin{bmatrix} x{k} \\ v_{k} \end{bmatrix} xk=[xkvk]，其中 x k x_{k} xk 是位置， v k v_{k} vk 是速度。
• 控制输入：假设没有控制输入，所以 u k = 0 \mathbf{u}_{k} = 0 uk=0。
• 状态转移矩阵： F = [ 1 Δ t 0 1 ] \mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} F=[10Δt1]，其中 Δ t \Delta t Δt 是时间间隔。
• 观测矩阵：假设我们只能测量位置， H = [ 1 0 ] \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} H=[10]。

矩阵乘法

1. 状态预测：
   x k ∣ k − 1 = F x k − 1 ∣ k − 1 = [ 1 Δ t 0 1 ] [ x k − 1 v k − 1 ] = [ x k − 1 + v k − 1 Δ t v k − 1 ] \mathbf{x}{k|k-1} = \mathbf{F} \mathbf{x}{k-1|k-1} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_{k-1} \\ v_{k-1} \end{bmatrix} = \begin{bmatrix} x_{k-1} + v_{k-1} \Delta t \\ v_{k-1} \end{bmatrix} xk∣k−1=Fxk−1∣k−1=[10Δt1][xk−1vk−1]=[xk−1+vk−1Δtvk−1]
2. 观测预测：
   z k ∣ k − 1 = H x k ∣ k − 1 = [ 1 0 ] [ x k − 1 + v k − 1 Δ t v k − 1 ] = x k − 1 + v k − 1 Δ t \mathbf{z}{k|k-1} = \mathbf{H} \mathbf{x}{k|k-1} = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} x_{k-1} + v_{k-1} \Delta t \\ v_{k-1} \end{bmatrix} = x_{k-1} + v_{k-1} \Delta t zk∣k−1=Hxk∣k−1=[10][xk−1+vk−1Δtvk−1]=xk−1+vk−1Δt

例子 2：二维位置和速度估计

假设我们有一个在平面上运动的物体，我们希望估计它的二维位置和速度。

状态空间模型

• 状态向量： x k = [ x k y k v x , k v y , k ] \mathbf{x}{k} = \begin{bmatrix} x{k} \\ y_{k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} xk= xkykvx,kvy,k ，其中 x k x_{k} xk 和 y k y_{k} yk 是位置， v x , k v_{x,k} vx,k 和 v y , k v_{y,k} vy,k 是速度。
• 控制输入：假设没有控制输入，所以 u k = 0 \mathbf{u}_{k} = 0 uk=0。
• 状态转移矩阵： F = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} F= 10000100Δt0100Δt01 。
• 观测矩阵：假设我们能测量位置， H = [ 1 0 0 0 0 1 0 0 ] \mathbf{H} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} H=[10010000]。

矩阵乘法

1. 状态预测：
   x k ∣ k − 1 = F x k − 1 ∣ k − 1 = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x k − 1 y k − 1 v x , k − 1 v y , k − 1 ] = [ x k − 1 + v x , k − 1 Δ t y k − 1 + v y , k − 1 Δ t v x , k − 1 v y , k − 1 ] \mathbf{x}{k|k-1} = \mathbf{F} \mathbf{x}{k-1|k-1} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} = \begin{bmatrix} x_{k-1} + v_{x,k-1} \Delta t \\ y_{k-1} + v_{y,k-1} \Delta t \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} xk∣k−1=Fxk−1∣k−1= 10000100Δt0100Δt01 xk−1yk−1vx,k−1vy,k−1 = xk−1+vx,k−1Δtyk−1+vy,k−1Δtvx,k−1vy,k−1
2. 观测预测：
   z k ∣ k − 1 = H x k ∣ k − 1 = [ 1 0 0 0 0 1 0 0 ] [ x k − 1 + v x , k − 1 Δ t y k − 1 + v y , k − 1 Δ t v x , k − 1 v y , k − 1 ] = [ x k − 1 + v x , k − 1 Δ t y k − 1 + v y , k − 1 Δ t ] \mathbf{z}{k|k-1} = \mathbf{H} \mathbf{x}{k|k-1} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} x_{k-1} + v_{x,k-1} \Delta t \\ y_{k-1} + v_{y,k-1} \Delta t \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} = \begin{bmatrix} x_{k-1} + v_{x,k-1} \Delta t \\ y_{k-1} + v_{y,k-1} \Delta t \end{bmatrix} zk∣k−1=Hxk∣k−1=[10010000] xk−1+vx,k−1Δtyk−1+vy,k−1Δtvx,k−1vy,k−1 =[xk−1+vx,k−1Δtyk−1+vy,k−1Δt]

物理运动方程转化为状态转移矩阵的形式

物理运动模型

假设我们有一个物体在二维平面上运动。我们希望用状态向量描述其位置和速度。假设物体的运动是匀速直线运动，以下是它的物理运动方程：

1. 位置更新：

• 在时间 Δ t \Delta t Δt 内，位置更新为：
  x k = x k − 1 + v x , k − 1 Δ t x_{k} = x_{k-1} + v_{x,k-1} \Delta t xk=xk−1+vx,k−1Δt
  y k = y k − 1 + v y , k − 1 Δ t y_{k} = y_{k-1} + v_{y,k-1} \Delta t yk=yk−1+vy,k−1Δt

2. 速度更新：

• 由于匀速直线运动，速度保持不变：
  v x , k = v x , k − 1 v_{x,k} = v_{x,k-1} vx,k=vx,k−1
  v y , k = v y , k − 1 v_{y,k} = v_{y,k-1} vy,k=vy,k−1

结合运动模型构造状态转移矩阵

状态定义

• 定义状态向量：
  x k = [ x k y k v x , k v y , k ] \mathbf{x}{k} = \begin{bmatrix} x{k} \\ y_{k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} xk= xkykvx,kvy,k
  其中 x k x_{k} xk 和 y k y_{k} yk 是位置， v x , k v_{x,k} vx,k 和 v y , k v_{y,k} vy,k 是速度。

状态方程

• 将物理运动模型的方程转化为状态方程：
  x k = F x k − 1 + w k \mathbf{x}{k} = \mathbf{F} \mathbf{x}{k-1} + \mathbf{w}{k} xk=Fxk−1+wk
  其中 F \mathbf{F} F 是状态转移矩阵， w k \mathbf{w}{k} wk 是过程噪声。

构造状态转移矩阵

• 根据上述物理运动模型，位置和速度的更新关系可以写成矩阵形式：
  [ x k y k v x , k v y , k ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x k − 1 y k − 1 v x , k − 1 v y , k − 1 ] \begin{bmatrix} x_{k} \\ y_{k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} xkykvx,kvy,k = 10000100Δt0100Δt01 xk−1yk−1vx,k−1vy,k−1
  这就构成了状态转移矩阵：
  F = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} F= 10000100Δt0100Δt01

解释状态转移矩阵

这个状态转移矩阵表示以下更新规则：

• 位置更新 ： x k = x k − 1 + v x , k − 1 Δ t x_{k} = x_{k-1} + v_{x,k-1} \Delta t xk=xk−1+vx,k−1Δt y k = y k − 1 + v y , k − 1 Δ t y_{k} = y_{k-1} + v_{y,k-1} \Delta t yk=yk−1+vy,k−1Δt
• 速度更新 ： v x , k = v x , k − 1 v_{x,k} = v_{x,k-1} vx,k=vx,k−1 v y , k = v y , k − 1 v_{y,k} = v_{y,k-1} vy,k=vy,k−1
  通过这种方式，状态转移矩阵 F \mathbf{F} F 将物理模型中位置和速度的更新关系转化为矩阵运算。这使得卡尔曼滤波器能够系统地处理状态预测和更新。

构造状态转移矩阵的过程

这种转换就是将多个线性方程组合成一个矩阵方程。具体步骤如下：

1. 定义状态向量

首先，我们需要定义系统的状态向量。状态向量包含了系统在每个时间步的所有状态变量。在二维位置和速度估计的例子中，状态向量定义为：
x k = [ x k y k v x , k v y , k ] \mathbf{x}{k} = \begin{bmatrix} x{k} \\ y_{k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} xk= xkykvx,kvy,k

2. 写出状态更新的方程组

根据物理模型，写出系统状态变量如何从时间 k − 1 k-1 k−1 变化到时间 k k k 的更新方程。对于匀速直线运动模型，我们有：
x k = x k − 1 + v x , k − 1 Δ t x_{k} = x_{k-1} + v_{x,k-1} \Delta t xk=xk−1+vx,k−1Δt
y k = y k − 1 + v y , k − 1 Δ t y_{k} = y_{k-1} + v_{y,k-1} \Delta t yk=yk−1+vy,k−1Δt
v x , k = v x , k − 1 v_{x,k} = v_{x,k-1} vx,k=vx,k−1
v y , k = v y , k − 1 v_{y,k} = v_{y,k-1} vy,k=vy,k−1

3. 将方程组转换为矩阵形式

为了将这些方程组表示为矩阵形式，我们可以使用线性代数中的矩阵乘法，将多个方程合并到一个矩阵方程中。我们希望找到一个矩阵 F \mathbf{F} F，使得下面的矩阵方程表示我们系统的状态更新：
x k = F x k − 1 \mathbf{x}{k} = \mathbf{F} \mathbf{x}{k-1} xk=Fxk−1

具体过程如下：

首先，写出状态更新方程的分量形式：

1. 对于位置 x k x_k xk 和速度 v x , k v_{x,k} vx,k：
   x k = 1 ⋅ x k − 1 + Δ t ⋅ v x , k − 1 + 0 ⋅ y k − 1 + 0 ⋅ v y , k − 1 x_{k} = 1 \cdot x_{k-1} + \Delta t \cdot v_{x,k-1} + 0 \cdot y_{k-1} + 0 \cdot v_{y,k-1} xk=1⋅xk−1+Δt⋅vx,k−1+0⋅yk−1+0⋅vy,k−1
   v x , k = 0 ⋅ x k − 1 + 1 ⋅ v x , k − 1 + 0 ⋅ y k − 1 + 0 ⋅ v y , k − 1 v_{x,k} = 0 \cdot x_{k-1} + 1 \cdot v_{x,k-1} + 0 \cdot y_{k-1} + 0 \cdot v_{y,k-1} vx,k=0⋅xk−1+1⋅vx,k−1+0⋅yk−1+0⋅vy,k−1
2. 对于位置 y k y_k yk 和速度 v y , k v_{y,k} vy,k：
   y k = 0 ⋅ x k − 1 + 0 ⋅ v x , k − 1 + 1 ⋅ y k − 1 + Δ t ⋅ v y , k − 1 y_{k} = 0 \cdot x_{k-1} + 0 \cdot v_{x,k-1} + 1 \cdot y_{k-1} + \Delta t \cdot v_{y,k-1} yk=0⋅xk−1+0⋅vx,k−1+1⋅yk−1+Δt⋅vy,k−1
   v y , k = 0 ⋅ x k − 1 + 0 ⋅ v x , k − 1 + 0 ⋅ y k − 1 + 1 ⋅ v y , k − 1 v_{y,k} = 0 \cdot x_{k-1} + 0 \cdot v_{x,k-1} + 0 \cdot y_{k-1} + 1 \cdot v_{y,k-1} vy,k=0⋅xk−1+0⋅vx,k−1+0⋅yk−1+1⋅vy,k−1
   接下来，我们可以将这些方程组合成矩阵形式：
   [ x k y k v x , k v y , k ] = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] [ x k − 1 y k − 1 v x , k − 1 v y , k − 1 ] \begin{bmatrix} x_{k} \\ y_{k} \\ v_{x,k} \\ v_{y,k} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ v_{x,k-1} \\ v_{y,k-1} \end{bmatrix} xkykvx,kvy,k = 10000100Δt0100Δt01 xk−1yk−1vx,k−1vy,k−1

4. 构造状态转移矩阵 F \mathbf{F} F

通过上面的矩阵形式，我们得到了状态转移矩阵 F \mathbf{F} F：
F = [ 1 0 Δ t 0 0 1 0 Δ t 0 0 1 0 0 0 0 1 ] \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} F= 10000100Δt0100Δt01