本文需要配合博客卡尔曼滤波详解进行理解
1.简单介绍
参考卡尔曼滤波详解
上面可简化理解为
2.主要过程
主要过程还是参考卡尔曼滤波详解
3.实例
这里以线性运动为例
3.1 前期定义状态和变量
3.1.1分析运动情况
已知线性运动上一状态和当前状态的关系，假设没有噪声干扰，为
{
x
′
=
x
+
v
x
Δ
t
y
′
=
y
+
v
y
Δ
t
\begin{cases} x'=x+v_x \Delta t \\ y'=y+v_y \Delta t \\ \end{cases}
{x′=x+vxΔty′=y+vyΔt
3.1.2 定义跟踪状态和状态转移矩阵
[
x
y
v
x
v
y
]
\left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right]
⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
则根据3.1.1可以得到状态转移关系
[
x
′
y
′
v
x
′
v
y
′
]
=
[
1
0
Δ
t
0
0
1
0
Δ
t
0
0
1
0
0
0
0
1
]
[
x
y
v
x
v
y
]
\left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right]
⎣⎢⎢⎡x′y′vx′vy′⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即
A
=
[
1
0
Δ
t
0
0
1
0
Δ
t
0
0
1
0
0
0
0
1
]
A=\left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]
A=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤
加上状态转移噪声
x
k
=
A
x
k
−
1
+
w
x_k=Ax_{k-1}+w
xk=Axk−1+w
3.1.3 定义状态到观测转移矩阵
由于在实际过程中，我们只观测到位置
[
z
x
z
y
]
\left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]
[zxzy]所以我们要得到从跟踪状态
[
x
y
v
x
v
y
]
\left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right]
⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤到
[
z
x
z
y
]
\left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]
[zxzy]的转换矩阵，假设我们观察的位置没有噪声干扰则：
[
z
x
z
y
]
=
[
1
0
0
0
0
1
0
0
]
[
x
y
v
x
v
y
]
\left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right]
[zxzy]=[10010000]⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即
H
=
[
1
0
0
0
0
1
0
0
]
H=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{matrix} \right]
H=[10010000]
加上测量噪声可得到
z
k
=
H
x
k
+
v
z_k=Hx_k+v
zk=Hxk+v
由上面三部我们就基本把所需要的变量定义完了，下面就需要进行预测和矫正了
3.4 卡尔曼滤波过程
假设我们来了一帧数据
[
z
x
z
y
t
]
\left[ \begin{matrix} z_x \\ z_y \\ t \\ \end{matrix} \right]
⎣⎡zxzyt⎦⎤
其中
[
z
x
z
y
]
\left[ \begin{matrix} z_x \\ z_y \\ \end{matrix} \right]
[zxzy]为观测数据，
t
t
t为当前时间，则进行下面步骤：
- (预测)根据 t t t和上一帧位置，预测当前帧位置，同时计算预测误差 P k ‾ \overline{P_k} Pk
- (矫正)求出卡尔曼增益，计算最优位置估计，计算当前估计误差 P k P_k Pk
3.2.1 预测
求出
Δ
t
=
t
−
t
p
r
e
\Delta t=t-t_{pre}
Δt=t−tpre，根据3.1.1估算当前位置
[
x
′
y
′
v
x
′
v
y
′
]
=
[
1
0
Δ
t
0
0
1
0
Δ
t
0
0
1
0
0
0
0
1
]
[
x
y
v
x
v
y
]
\left[ \begin{matrix} x' \\ y' \\ v_x' \\ v_y' \\ \end{matrix} \right]= \left[ \begin{matrix} 1 & 0 & \Delta t & 0\\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} x \\ y \\ v_x \\ v_y \\ \end{matrix} \right]
⎣⎢⎢⎡x′y′vx′vy′⎦⎥⎥⎤=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤⎣⎢⎢⎡xyvxvy⎦⎥⎥⎤
即
x
k
^
‾
=
A
x
k
−
1
^
\overline{\hat{x_k}}=A\hat{x_{k-1}}
xk^=Axk−1^
计算当前估计误差，结合基本公式(过程噪声
Q
Q
Q)
P
k
‾
=
A
P
k
−
1
A
T
+
Q
\overline{P_k}=AP_{k-1}A^T+Q
Pk=APk−1AT+Q
3.2.2 矫正
结合基本公式
计算卡尔曼增益(测量噪声
R
R
R)
K
k
=
P
k
‾
H
T
(
H
P
k
‾
H
T
+
R
)
−
1
K_k=\overline{P_k}H^T(H\overline{P_k}H^T+R)^{-1}
Kk=PkHT(HPkHT+R)−1
计算最优位置估计
x
k
^
=
x
k
^
‾
+
K
k
(
z
k
−
H
x
k
^
‾
)
\hat{x_k}=\overline{\hat{x_k}}+K_k(z_k-H\overline{\hat{x_k}})
xk^=xk^+Kk(zk−Hxk^)
计算估计误差
P
k
=
(
I
−
K
k
H
)
P
k
‾
P_k=(I-K_kH)\overline{P_k}
Pk=(I−KkH)Pk
3.2.3 实际过程
综上，我们在实际过程中，只要设置好
- 跟踪变量 x x x
- 状态转移矩阵 A A A
- 状态到观测矩阵 H H H
- 状态转移噪声 Q Q Q
- 测量噪声 R R R
- 初始估计误差 P k P_k Pk(设置不能太小)
- 第一次需要设置
x
k
^
\hat{x_k}
xk^的值
就可以了
5352
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
