DeepSORT（目标跟踪算法） 卡尔曼滤波 状态空间模型的理解
DeepSORT（目标跟踪算法） 卡尔曼滤波 状态空间模型的理解
flyfish
卡尔曼滤波器是一种用于估计动态系统状态的递归算法，它基于状态空间模型进行工作。状态空间模型由两个主要方程组成：状态方程和观测方程。
状态方程描述系统的状态如何随时间演变，而观测方程描述如何通过测量值观察到系统的状态。
状态空间模型可以表示为：
- 状态方程：
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 - 观测方程：
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]。
矩阵乘法
- 状态预测：
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] - 观测预测：
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]。
矩阵乘法
- 状态预测：
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
- 观测预测：
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]
物理运动方程转化为状态转移矩阵的形式
物理运动模型
假设我们有一个物体在二维平面上运动。我们希望用状态向量描述其位置和速度。假设物体的运动是匀速直线运动，以下是它的物理运动方程：
- 位置更新：
- 在时间
Δ
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
- 速度更新：
- 由于匀速直线运动，速度保持不变：
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Δty
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−1v
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
具体过程如下：
首先，写出状态更新方程的分量形式：
- 对于位置
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 - 对于位置
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
