Kalman滤波推导复习之1
复习Kalman滤波之推导。
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
z
k
=
H
x
k
+
v
k
\begin{aligned} \bm{x}_k = \bm{A}\bm{x}_{k-1} + \bm{B}\bm{u}_{k-1} + \bm{w}_{k-1}\\ \bm{z}_k = \bm{H}\bm{x}_{k} + \bm{v}_{k} \end{aligned}
xk=Axk−1+Buk−1+wk−1zk=Hxk+vk
因为噪声变幻莫测,所以我们给出简化版本的公式:
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
x
^
k
,
m
e
a
s
u
r
e
=
H
−
1
z
k
\begin{aligned} \hat{\bm{x}}^-_k = \bm{A}\hat{\bm{x}}_{k-1} + \bm{B}\bm{u}_{k-1}\\ \hat{\bm{x}}_{k,measure} = \bm{H}^{-1}\bm{z}_k \end{aligned}
x^k−=Ax^k−1+Buk−1x^k,measure=H−1zk
数据融合一下:
x
^
k
=
x
^
k
−
+
G
(
x
^
k
,
m
e
a
s
u
r
e
−
x
^
k
−
)
=
x
^
k
−
+
G
(
H
−
1
z
k
−
x
^
k
−
)
\begin{aligned} \hat{\bm{x}}_k = \hat{\bm{x}}^-_k + \bm{G} \left( \hat{\bm{x}}_{k,measure} - \hat{\bm{x}}^-_k \right) = \hat{\bm{x}}^-_k + \bm{G} \left( \bm{H}^{-1}\bm{z}_k - \hat{\bm{x}}^-_k \right) \end{aligned}
x^k=x^k−+G(x^k,measure−x^k−)=x^k−+G(H−1zk−x^k−)
定义
G
=
K
H
\bm{G} = \bm{K}\bm{H}
G=KH则有:
x
^
k
=
x
^
k
−
+
K
(
z
k
−
H
x
^
k
−
)
\begin{aligned} \boxed{\hat{\bm{x}}_k = \hat{\bm{x}}^-_k + \bm{K} \left( \bm{z}_k - \bm{H} \hat{\bm{x}}^-_k \right)} \end{aligned}
x^k=x^k−+K(zk−Hx^k−)
目标函数为:
min
ℓ
=
T
r
[
E
(
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
⊤
)
]
=
T
r
[
E
(
(
x
k
−
x
^
k
−
−
K
(
z
k
−
H
x
^
k
−
)
)
(
x
k
−
x
^
k
−
−
K
(
z
k
−
H
x
^
k
−
)
)
⊤
)
]
=
T
r
[
E
(
(
x
k
−
x
^
k
−
−
K
(
H
x
k
+
v
k
−
H
x
^
k
−
)
)
(
x
k
−
x
^
k
−
−
K
(
H
x
k
+
v
k
−
H
x
^
k
−
)
)
⊤
)
]
\begin{aligned} \min \ell &= {\rm Tr} \left[\mathbb{E}\left ((\bm{x}_k - \hat{\bm{x}}_k)(\bm{x}_k - \hat{\bm{x}}_k)^{\top} \right) \right]\\ & ={\rm Tr} \left[\mathbb{E}\left ( \left(\bm{x}_k - \hat{\bm{x}}^-_k - \bm{K} \left( \bm{z}_k - \bm{H} \hat{\bm{x}}^-_k \right) \right) \left(\bm{x}_k - \hat{\bm{x}}^-_k - \bm{K} \left( \bm{z}_k - \bm{H} \hat{\bm{x}}^-_k \right) \right)^{\top} \right) \right]\\ & = {\rm Tr} \left[\mathbb{E}\left ( \left(\bm{x}_k - \hat{\bm{x}}^-_k - \bm{K} \left( \bm{H}\bm{x}_{k} + \bm{v}_{k} - \bm{H} \hat{\bm{x}}^-_k \right) \right) \left(\bm{x}_k - \hat{\bm{x}}^-_k - \bm{K} \left( \bm{H}\bm{x}_{k} + \bm{v}_{k} - \bm{H} \hat{\bm{x}}^-_k \right) \right)^{\top} \right) \right] \end{aligned}
minℓ=Tr[E((xk−x^k)(xk−x^k)⊤)]=Tr[E((xk−x^k−−K(zk−Hx^k−))(xk−x^k−−K(zk−Hx^k−))⊤)]=Tr[E((xk−x^k−−K(Hxk+vk−Hx^k−))(xk−x^k−−K(Hxk+vk−Hx^k−))⊤)]
继续快乐的化简之:
min
ℓ
=
T
r
[
E
(
(
(
I
−
K
H
)
(
x
k
−
x
^
k
−
)
−
K
v
k
)
(
(
I
−
K
H
)
(
x
k
−
x
^
k
−
)
−
K
v
k
)
⊤
)
]
\begin{aligned} \min \ell = {\rm Tr} \left[\mathbb{E}\left ( \left( (\bm{I} - \bm{K}\bm{H})(\bm{x}_k - \hat{\bm{x}}^-_k) - \bm{K}\bm{v}_k \right) \left( (\bm{I} - \bm{K}\bm{H})(\bm{x}_k - \hat{\bm{x}}^-_k) - \bm{K}\bm{v}_k \right)^{\top} \right) \right] \end{aligned}
minℓ=Tr[E(((I−KH)(xk−x^k−)−Kvk)((I−KH)(xk−x^k−)−Kvk)⊤)]
定义
e
k
=
x
k
−
x
^
k
−
\bm{e}_k = \bm{x}_k - \hat{\bm{x}}^-_k
ek=xk−x^k−,则有:
min
ℓ
=
T
r
[
E
(
(
(
I
−
K
H
)
e
k
−
K
v
k
)
(
(
I
−
K
H
)
e
k
−
K
v
k
)
⊤
)
]
=
T
r
[
E
(
(
(
I
−
K
H
)
e
k
−
K
v
k
)
(
e
k
⊤
(
I
−
K
H
)
⊤
−
v
k
⊤
K
⊤
)
)
]
=
T
r
[
(
I
−
K
H
)
E
(
e
k
e
k
⊤
)
(
I
−
K
H
)
⊤
]
+
T
r
[
K
E
(
v
k
v
k
⊤
)
K
⊤
]
\begin{aligned} \min \ell &= {\rm Tr} \left[\mathbb{E}\left ( \left( (\bm{I} - \bm{K}\bm{H})\bm{e}_k - \bm{K}\bm{v}_k \right) \left( (\bm{I} - \bm{K}\bm{H})\bm{e}_k - \bm{K}\bm{v}_k \right)^{\top} \right) \right]\\ & = {\rm Tr} \left[\mathbb{E}\left ( \left( (\bm{I} - \bm{K}\bm{H})\bm{e}_k - \bm{K}\bm{v}_k \right) \left( \bm{e}_k^{\top} (\bm{I} - \bm{K}\bm{H})^{\top} - \bm{v}_k^{\top}\bm{K}^{\top} \right) \right) \right]\\ & = {\rm Tr} \left[ (\bm{I} - \bm{K}\bm{H}) \mathbb{E} \left(\bm{e}_k\bm{e}_k^{\top} \right) (\bm{I} - \bm{K}\bm{H})^{\top} \right] + {\rm Tr} \left[ \bm{K} \mathbb{E}(\bm{v}_k \bm{v}_k^{\top})\bm{K}^{\top}\right] \end{aligned}
minℓ=Tr[E(((I−KH)ek−Kvk)((I−KH)ek−Kvk)⊤)]=Tr[E(((I−KH)ek−Kvk)(ek⊤(I−KH)⊤−vk⊤K⊤))]=Tr[(I−KH)E(ekek⊤)(I−KH)⊤]+Tr[KE(vkvk⊤)K⊤]
值得注意的是:
E
(
e
k
)
=
E
(
v
k
)
=
0
E
(
e
k
v
k
⊤
)
=
0.
\begin{aligned} \mathbb{E} \left(\bm{e}_k\right) = \mathbb{E} \left(\bm{v}_k\right) = 0\\ \mathbb{E} \left(\bm{e}_k\bm{v}_k^{\top}\right) = 0. \end{aligned}
E(ek)=E(vk)=0E(ekvk⊤)=0.
如果令$ \mathbb{E} \left(\bm{e}_k\bm{e}_k^{\top} \right) = \bm{P}_k
并
且
并且
并且 \mathbb{E} \left(\bm{v}_k\bm{v}_k^{\top} \right) = \bm{R}$:
min
ℓ
=
T
r
[
(
I
−
K
H
)
P
k
(
I
−
K
H
)
⊤
]
+
T
r
[
K
R
K
⊤
]
\begin{aligned} \min \ell = {\rm Tr} \left[ (\bm{I} - \bm{K}\bm{H}) \bm{P}_k (\bm{I} - \bm{K}\bm{H})^{\top} \right] + {\rm Tr} \left[ \bm{K} \bm{R} \bm{K}^{\top}\right] \end{aligned}
minℓ=Tr[(I−KH)Pk(I−KH)⊤]+Tr[KRK⊤]
现在的目标是得到
K
\bm{K}
K了:
∂
ℓ
∂
K
=
0
\begin{aligned} \frac{\partial \ell}{\partial \bm{K}} = 0 \end{aligned}
∂K∂ℓ=0
容易解得:
K
=
P
k
H
⊤
H
P
k
H
⊤
+
R
\begin{aligned} \boxed{\bm{K} = \frac{\bm{P}_k\bm{H}^{\top}}{\bm{H}\bm{P}_k\bm{H}^{\top} + \bm{R}}} \end{aligned}
K=HPkH⊤+RPkH⊤
分析一下:
R
↑
⇒
K
→
0.
\begin{aligned} \bm{R} \uparrow \, \Rightarrow \bm{K} \rightarrow 0. \end{aligned}
R↑⇒K→0.
也就是测量误差很大的时候,模型倾向于相信建模得到的结果。
反之:
R
↓
⇒
K
→
H
−
1
.
\begin{aligned} \bm{R} \downarrow \, \Rightarrow \bm{K} \rightarrow \bm{H}^{-1}. \end{aligned}
R↓⇒K→H−1.
模型倾向于相信测量得到的结果。