秋招要到了，复习一下卡尔曼滤波的推导过程。
(http://blog.csdn.net/heyijia0327）
《视觉SLAM14讲》
假设系统为线性高斯系统，即SLAM的运动方程和观测方程都可以由线性方程来表示，即：
{
x
k
=
A
k
x
k
−
1
+
u
k
+
w
k
z
k
=
C
k
x
k
+
v
k
,
k
=
1
,
.
.
.
,
N
\left\{
假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布：
w
k
∼
N
(
0
,
R
)
,
v
k
∼
N
(
0
,
Q
)
\left. \mathbf{w}_{k} \right.\sim\left. N\left( {0,\mathbf{R}} \right),~\mathbf{v}_{k} \right.\sim N\left( {0,\mathbf{Q}} \right)
wk∼N(0,R), vk∼N(0,Q) 。以尖帽子
x
^
k
\hat{\boldsymbol{x}}_k
x^k 表示后验，以横线
x
ˉ
k
\bar{\boldsymbol{x}}_k
xˉk 表示先验分布。
第一步：预测。根据运动方程确定
x
k
\boldsymbol{x}_k
xk 的先验分布。
P
(
x
k
|
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
=
N
(
A
k
x
^
k
−
1
+
u
k
,
A
k
P
^
k
−
1
A
k
T
+
R
)
P\left( \mathbf{x}_{k} \middle| \mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k - 1} \right) = N\left( {\mathbf{A}_{k}{\hat{\mathbf{x}}}_{k - 1} + \mathbf{u}_{k},\mathbf{A}_{k}{\hat{\mathbf{P}}}_{k - 1}\mathbf{A}_{k}^{T} + \mathbf{R}} \right)
P(xk∣x0,u1:k,z1:k−1)=N(Akx^k−1+uk,AkP^k−1AkT+R)
即：
x
ˉ
k
=
A
k
x
^
k
−
1
+
u
k
P
ˉ
k
=
A
k
P
^
k
−
1
A
k
T
+
R
\bar{\mathbf{x}}_{k} = \bold{A}_k \hat{\mathbf{x}}_{k - 1} + \mathbf{u}_{k} \\ \bar{\mathbf{P}}_{k} =\bold{A}_k \hat{\mathbf{P}}_{k - 1} \bold{A}_k^T + \mathbf{R}
xˉk=Akx^k−1+ukPˉk=AkP^k−1AkT+R
这一步的意思是，从上一个时刻的状态，根据输入信息（但是有噪声），推断当前时刻的状态分布。这个分布也就是先验。
第二步，先计算卡尔曼增益
K
\bold{K}
K，再计算后验。由观测方程，我们可以计算在某个状态下，应该产生怎样的观测数据：
P
(
z
k
|
x
k
)
=
N
(
C
k
x
k
,
Q
)
P\left( \mathbf{z}_{k} \middle| \mathbf{x}_{k} \right) = N\left( {\mathbf{C}_{k}\mathbf{x}_{k},\mathbf{Q}} \right)
P(zk∣xk)=N(Ckxk,Q)
为了计算
x
^
k
\hat{\boldsymbol{x}}_k
x^k ，根据贝叶斯公式:
P
(
x
k
|
x
0
,
u
1
:
k
,
z
1
:
k
)
∝
P
(
z
k
|
x
k
)
P
(
x
k
|
x
0
,
u
1
:
k
,
z
1
:
k
−
1
)
P\left( \mathbf{x}_{k} \middle| \mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k} \right) \propto P\left( \mathbf{z}_{k} \middle| \mathbf{x}_{k} \right)P\left( \mathbf{x}_{k} \middle| \mathbf{x}_{0},\mathbf{u}_{1:k},\mathbf{z}_{1:k - 1} \right)
P(xk∣x0,u1:k,z1:k)∝P(zk∣xk)P(xk∣x0,u1:k,z1:k−1)
有:
N
(
x
^
k
,
P
^
k
)
=
N
(
C
k
x
k
,
Q
)
⋅
N
(
x
ˉ
k
,
P
ˉ
k
)
N\left( {{\hat{\mathbf{x}}}_{k},{\hat{\mathbf{P}}}_{k}} \right) = N\left( {\mathbf{C}_{k}\mathbf{x}_{k},\mathbf{Q}} \right) \cdot N\left( \bar{\mathbf{x}}_{k},\bar{\mathbf{P}}_{k} \right)
N(x^k,P^k)=N(Ckxk,Q)⋅N(xˉk,Pˉk)
既然我们已经知道等式两侧都是高斯分布，那就只需比较指数部分即可。指数部分很像是一个二次型的配方，我们来推导一下。首先把指数部分展开，有:
(
x
k
−
x
^
k
)
T
P
^
k
−
1
(
x
k
−
x
^
k
)
=
(
z
k
−
C
k
x
k
)
T
Q
−
1
(
z
k
−
C
k
x
k
)
+
(
x
k
−
x
ˉ
k
)
T
P
ˉ
k
−
1
(
x
k
−
x
ˉ
k
)
\left( {\mathbf{x}_{k} - {\hat{\mathbf{x}}}_{k}} \right)^{T}{\hat{\mathbf{P}}}_{k}^{- 1}\left( {\mathbf{x}_{k} - {\hat{\mathbf{x}}}_{k}} \right) \\= \left( {\mathbf{z}_{k} - \mathbf{C}_{k}\mathbf{x}_{k}} \right)^{T}\mathbf{Q}_{~}^{- 1}\left( {\mathbf{z}_{k} - \mathbf{C}_{k}\mathbf{x}_{k}} \right) + \left( \mathbf{x}_{k} - \bar{\mathbf{x}}_{k} \right)^{T} {\bar{\mathbf{P}}}_{k}^{- 1} \left( \mathbf{x}_{k} - \bar{\mathbf{x}}_{k} \right)
(xk−x^k)TP^k−1(xk−x^k)=(zk−Ckxk)TQ −1(zk−Ckxk)+(xk−xˉk)TPˉk−1(xk−xˉk)
为了求左侧的
x
^
k
\hat{\boldsymbol{x}}_k
x^k 和
P
^
k
\hat{\boldsymbol{P}}_k
P^k ，我们把两边展开，并比较
x
k
{\boldsymbol{x}}_k
xk 的二次和一次系数。
左侧：
(
x
k
−
x
^
k
)
T
P
^
k
−
1
(
x
k
−
x
^
k
)
=
(
x
k
T
−
x
^
k
T
)
(
P
^
k
−
1
x
k
−
P
^
k
−
1
x
^
k
)
=
x
k
T
P
^
k
−
1
x
k
−
x
k
T
P
^
k
−
1
x
^
k
−
x
^
k
T
P
^
k
−
1
x
k
+
x
^
k
T
P
^
k
−
1
x
^
k
\left( {\mathbf{x}_{k} - {\hat{\mathbf{x}}}_{k}} \right)^{T}{\hat{\mathbf{P}}}_{k}^{- 1}\left( {\mathbf{x}_{k} - {\hat{\mathbf{x}}}_{k}} \right) = \left( {\mathbf{x}_{k}^{T} - {\hat{\mathbf{x}}}_{k}^{T}} \right)\left( {{\hat{\mathbf{P}}}_{k}^{- 1}\mathbf{x}_{k} - {\hat{\mathbf{P}}}_{k}^{- 1}{\hat{\mathbf{x}}}_{k}} \right) \\ = \mathbf{x}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1}\mathbf{x}_{k} - \mathbf{x}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1}{\hat{\mathbf{x}}}_{k} - {\hat{\mathbf{x}}}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1}\mathbf{x}_{k} + {\hat{\mathbf{x}}}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1}{\hat{\mathbf{x}}}_{k}
(xk−x^k)TP^k−1(xk−x^k)=(xkT−x^kT)(P^k−1xk−P^k−1x^k)=xkTP^k−1xk−xkTP^k−1x^k−x^kTP^k−1xk+x^kTP^k−1x^k
右侧：
( z k − C k x k ) T Q − 1 ( z k − C k x k ) + ( x k − x ˉ k ) T P ˉ k − 1 ( x k − x ˉ k ) \left( {\mathbf{z}_{k} - \mathbf{C}_{k}\mathbf{x}_{k}} \right)^{T}\mathbf{Q}_{~}^{- 1}\left( {\mathbf{z}_{k} - \mathbf{C}_{k}\mathbf{x}_{k}} \right) + \left( {\mathbf{x}_{k} - {\bar{\mathbf{x}}}_{k}} \right)^{T} {\bar{\mathbf{P}}}_{k}^{- 1} \left( {\mathbf{x}_{k} - {\bar{\mathbf{x}}}_{k} } \right) (zk−Ckxk)TQ −1(zk−Ckxk)+(xk−xˉk)TPˉk−1(xk−xˉk)
对于二次系数，有：
P
^
k
−
1
=
C
k
T
Q
−
1
C
k
+
P
ˉ
k
−
1
{\hat{\mathbf{P}}}_{k}^{- 1} = \mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{C}_{k} + {\bar{\mathbf{P}}}_{k}^{- 1}
P^k−1=CkTQ −1Ck+Pˉk−1
该式给出了协方差的计算过程。定义卡尔曼增益：
K
=
P
^
k
C
k
T
Q
−
1
\mathbf{K} = {\hat{\mathbf{P}}}_{k}^{~}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}
K=P^k CkTQ −1
则
P
^
k
\hat{\bold{P}}_k
P^k 的推导为：
P
^
k
−
1
=
C
k
T
Q
−
1
C
k
+
P
ˉ
k
−
1
⇒
I
=
P
^
k
C
k
T
Q
−
1
C
k
+
P
^
k
P
ˉ
k
−
1
⇒
I
=
K
C
k
+
P
^
k
P
ˉ
k
−
1
⇒
P
ˉ
k
=
K
C
k
P
ˉ
k
+
P
^
k
⇒
P
^
k
=
(
I
−
K
C
k
)
P
ˉ
k
{\hat{\mathbf{P}}}_{k}^{- 1} = \mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{C}_{k} + {\bar{\mathbf{P}}}_{k}^{- 1} \\ \Rightarrow\mathbf{I} = {\hat{\mathbf{P}}}_{k}^{~}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{C}_{k} + {\hat{\mathbf{P}}}_{k}^{~}{\bar{\mathbf{P}}}_{k}^{- 1} \\ \Rightarrow\mathbf{I} = \mathbf{K}\mathbf{C}_{k} + {\hat{\mathbf{P}}}_{k}^{~}{\bar{\mathbf{P}}}_{k}^{- 1} \\ \Rightarrow {\bar{\mathbf{P}}}_{k} = \mathbf{K}\mathbf{C}_{k}{\bar{\mathbf{P}}}_{k} + {\hat{\mathbf{P}}}_{k}^{~} \\ \Rightarrow{\hat{\mathbf{P}}}_{k}^{~} = \left( {\mathbf{I} - \mathbf{K}\mathbf{C}_{k}} \right) {\bar{\mathbf{P}}}_{k}
P^k−1=CkTQ −1Ck+Pˉk−1⇒I=P^k CkTQ −1Ck+P^k Pˉk−1⇒I=KCk+P^k Pˉk−1⇒Pˉk=KCkPˉk+P^k ⇒P^k =(I−KCk)Pˉk
K
\bold{K}
K 的推导：
K
=
P
^
k
C
k
T
Q
−
1
=
(
I
−
K
C
k
)
P
ˉ
k
C
k
T
Q
−
1
⇒
K
=
P
ˉ
k
C
k
T
Q
−
1
−
K
C
k
P
ˉ
k
C
k
T
Q
−
1
⇒
K
(
I
+
C
k
P
ˉ
k
C
k
T
Q
−
1
)
=
P
ˉ
k
C
k
T
Q
−
1
⇒
K
(
Q
+
C
k
P
ˉ
k
C
k
T
)
=
P
ˉ
k
C
k
T
⇒
K
=
P
ˉ
k
C
k
T
(
Q
+
C
k
P
ˉ
k
C
k
T
)
−
1
\mathbf{K} = {\hat{\mathbf{P}}}_{k}^{~}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1} = \left( {\mathbf{I} - \mathbf{K}\mathbf{C}_{k}} \right){\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1} \\ \Rightarrow\mathbf{K} = {\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1} - \mathbf{K}\mathbf{C}_{k}{\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1} \\ \Rightarrow\mathbf{K}\left( {\mathbf{I} + \mathbf{C}_{k}{\bar{\mathbf{P}}}_{k} \mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}} \right) = {\bar{\mathbf{P}}}_{k} \mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1} \\ \Rightarrow\mathbf{K}\left( {\mathbf{Q} + \mathbf{C}_{k}{\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}} \right) = {\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T} \\ \Rightarrow\mathbf{K} = {\bar{\mathbf{P}}}_{k} \mathbf{C}_{k}^{T}\left( {\mathbf{Q} + \mathbf{C}_{k}{\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}} \right)^{- 1}
K=P^k CkTQ −1=(I−KCk)PˉkCkTQ −1⇒K=PˉkCkTQ −1−KCkPˉkCkTQ −1⇒K(I+CkPˉkCkTQ −1)=PˉkCkTQ −1⇒K(Q+CkPˉkCkT)=PˉkCkT⇒K=PˉkCkT(Q+CkPˉkCkT)−1
对于一次项系数，有：
−
2
x
^
k
T
P
^
k
−
1
x
k
=
−
2
z
k
T
Q
−
1
C
k
x
k
−
2
x
ˉ
k
T
P
ˉ
k
−
1
x
k
⇒
x
^
k
T
P
^
k
−
1
=
z
k
T
Q
−
1
C
k
+
x
ˉ
k
T
P
ˉ
k
−
1
⇒
P
^
k
−
1
x
^
k
=
C
k
T
Q
−
1
z
k
+
P
ˉ
k
−
1
x
ˉ
k
⇒
x
^
k
=
P
^
k
C
k
T
Q
−
1
z
k
+
P
^
k
P
ˉ
k
−
1
x
ˉ
k
-2{\hat{\mathbf{x}}}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1}\mathbf{x}_{k} = - 2\mathbf{z}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{C}_{k}\mathbf{x}_{k} - 2{\bar{\mathbf{x}}}_{k}^{T}{\bar{\mathbf{P}}}_{k}^{- 1}\mathbf{x}_{k} \\ \Rightarrow{\hat{\mathbf{x}}}_{k}^{T}{\hat{\mathbf{P}}}_{k}^{- 1} = \mathbf{z}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{C}_{k} + {\bar{\mathbf{x}}}_{k}^{T}{\bar{\mathbf{P}}}_{k}^{- 1} \\ \Rightarrow{\hat{\mathbf{P}}}_{k}^{- 1}{\hat{\mathbf{x}}}_{k}^{~} = \mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{z}_{k}^{~} + {\bar{\mathbf{P}}}_{k}^{- 1}{\bar{\mathbf{x}}}_{k} \\ \Rightarrow{\hat{\mathbf{x}}}_{k}^{~} = {\hat{\mathbf{P}}}_{k}^{~}\mathbf{C}_{k}^{T}\mathbf{Q}_{~}^{- 1}\mathbf{z}_{k}^{~} + {\hat{\mathbf{P}}}_{k}^{~}{\bar{\mathbf{P}}}_{k}^{- 1}{\bar{\mathbf{x}}}_{k}
−2x^kTP^k−1xk=−2zkTQ −1Ckxk−2xˉkTPˉk−1xk⇒x^kTP^k−1=zkTQ −1Ck+xˉkTPˉk−1⇒P^k−1x^k =CkTQ −1zk +Pˉk−1xˉk⇒x^k =P^k CkTQ −1zk +P^k Pˉk−1xˉk
代入卡尔曼增益：
x ^ k = K z k + P ^ k P ˉ k − 1 x ˉ k = K z k + ( I − K C k ) x ˉ k = x ˉ k + K ( z k − C k x ˉ k ) {\hat{\mathbf{x}}}_{k}^{~} = \mathbf{K}\mathbf{z}_{k}^{~} + {\hat{\mathbf{P}}}_{k}^{~}{\bar{\mathbf{P}}}_{k}^{- 1}{\bar{\mathbf{x}}}_{k} \\ = \mathbf{K}\mathbf{z}_{k}^{~} + \left( {\mathbf{I} - \mathbf{K}\mathbf{C}_{k}} \right){\bar{\mathbf{x}}}_{k} \\ = {\bar{\mathbf{x}}}_{k} + \mathbf{K}\left( {\mathbf{z}_{k}^{~} - \mathbf{C}_{k}{\bar{\mathbf{x}}}_{k}} \right) x^k =Kzk +P^k Pˉk−1xˉk=Kzk +(I−KCk)xˉk=xˉk+K(zk −Ckxˉk)
x ˉ k = A k x ^ k − 1 + u k P ˉ k = A k P ^ k − 1 A k T + R {\bar{\mathbf{x}}}_{k} = \mathbf{A}_{k}{\hat{\mathbf{x}}}_{k - 1} + \mathbf{u}_{k} \\ {\bar{\mathbf{P}}}_{k} = \mathbf{A}_{k}{\hat{\mathbf{P}}}_{k - 1}\mathbf{A}_{k}^{T} + \mathbf{R} xˉk=Akx^k−1+ukPˉk=AkP^k−1AkT+R
K = P ˉ k C k T ( C k P ˉ k C k T + Q ) − 1 \mathbf{K} = {\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T}\left( {\mathbf{C}_{k}{\bar{\mathbf{P}}}_{k}\mathbf{C}_{k}^{T} + \mathbf{Q}} \right)_{~}^{- 1} K=PˉkCkT(CkPˉkCkT+Q) −1
x ^ k = x ˉ k + K ( z k − C k x ˉ k ) P ^ k = ( I − K C k ) P ˉ k {\hat{\mathbf{x}}}_{k} = {\bar{\mathbf{x}}}_{k} + \mathbf{K}\left( {\mathbf{z}_{k} - \mathbf{C}_{k}{\bar{\mathbf{x}}}_{k}^{~}} \right) \\ {\hat{\mathbf{P}}}_{k}^{~} = \left( {\mathbf{I} - \mathbf{K}\mathbf{C}_{k}} \right){\bar{\mathbf{P}}}_{k}^{~} x^k=xˉk+K(zk−Ckxˉk )P^k =(I−KCk)Pˉk