虽然如今的SLAM / VIO已经不太可能再使用卡尔曼滤波作整体的紧耦合优化，但其分析问题的思想却深深影响着现如今基于优化的方法，同时读完《State Estimation for Robotics》后发现之前早期的理解很不到位，导致在之前做扫地机器人时Kalman模块对精度并没有什么影响。所以写下这篇文章加以总结。文章将先从理论推导说起，然后实现基于卡尔曼滤波的鼠标跟踪（c++）。贺博之前实现了python的版本。
回顾一下线性状态下，基于高斯分布的机器人状态估计的经典公式:
x
k
=
A
k
−
1
x
k
−
1
+
B
v
k
+
w
k
y
k
=
C
k
x
k
+
n
k
x_{k} = A_{k-1}x_{k-1} + Bv_{k} +w_{k}\\ y_{k} = C_{k} x_k + n_{k}\\
xk=Ak−1xk−1+Bvk+wkyk=Ckxk+nk
x 0 ∼ N ( x 0 ˇ , P 0 ˇ ) w k ∼ N ( 0 , Q k ) n k ∼ N ( 0 , R k ) x_0 \sim\mathcal N(\check{x_0},\check{P_0})\\ w_k \sim\mathcal N(0,Q_k)\\ n_k \sim \mathcal N(0,R_k) x0∼N(x0ˇ,P0ˇ)wk∼N(0,Qk)nk∼N(0,Rk)
为方便理解，首先明确卡尔曼滤波器的目标是，估计当前状态
x
k
\ x_{k}
xk满足什么样的高斯分布
N
(
x
k
^
,
P
k
^
)
\mathcal N(\hat{x_k},\hat{P_k})
N(xk^,Pk^)（为什么有这样的目标——是利用了高斯模型经过线性变换后依旧为高斯的性质）。值得一提的是一般将预测状态称为先验，用下箭头
x
k
ˇ
\check{x_{k}}
xkˇ表示，经观测修正后的状态为后验，用上箭头表示
x
k
^
\hat{x_{k}}
xk^。
滤波器根据状态转移公式以及上一时刻分布{
x
k
−
1
^
,
P
k
−
1
^
\hat{x_{k-1}},\hat{P_{k-1}}
xk−1^,Pk−1^}获得预测当前状态
x
k
ˇ
\check{x_k}
xkˇ,这个先验被认为存在很大误差，且不确定度较高，同一时刻，滤波器又获得了相关的测量信息，综合上述定义，将问题转换成如何根据测量值减小先验
x
k
ˇ
\check{x_k}
xkˇ的误差与不确定度，推出后验
x
k
^
\hat{x_k}
xk^的分布，即均值与方差?那么假设当前真值
x
k
˙
\dot{x_k}
xk˙满足高斯分布，那么它当前的均值和方差为{
x
k
^
,
P
k
^
\hat{x_k},\hat{P_k}
xk^,Pk^}:
x
k
˙
=
x
k
^
+
M
k
M
k
∼
N
(
0
,
P
k
^
)
\dot{x_k} = \hat{x_k} + M_k\\ M_k \sim\mathcal N(0,\hat{P_k})\\
xk˙=xk^+MkMk∼N(0,Pk^)
则有如下目标：
求
P
(
x
k
˙
)
=
P
(
x
k
^
∣
x
0
ˇ
,
v
1
:
k
,
y
0
:
k
)
=
P
(
x
k
^
∣
x
k
−
1
ˇ
,
v
k
,
y
k
)
分
布
求P(\dot{x_k}) = P(\hat{x_k}|\check{x_{0}},v_{1:k},y_{0:k}) =P(\hat{x_k}|\check{x_{k-1}},v_k,y_k) 分布
求P(xk˙)=P(xk^∣x0ˇ,v1:k,y0:k)=P(xk^∣xk−1ˇ,vk,yk)分布
可以通过三种不同方式进行推导：
贝叶斯直接求
直接求方差和协方差，即可获得递推式:
MAP转换成最小二乘问题
而从map角度出发对于每一个先验
x
k
ˇ
\check{x_k}
xkˇ，都可以由观测函数得出估计测量:
z
k
ˇ
=
C
k
x
k
ˇ
\check{z_k} = C_k\check{x_k}
zkˇ=Ckxkˇ
最小化当前测量与预估的差值便成了目标函数:
m
i
n
(
z
k
−
z
k
ˇ
)
2
\ min(z_k-\check{z_k})^2
min(zk−zkˇ)2，将目标函数展开写成矩阵的形式：
m
i
n
(
z
k
−
z
k
ˇ
)
=
m
i
n
J
(
x
)
=
m
i
n
1
2
(
z
−
H
x
)
T
W
−
1
(
z
−
H
x
)
z
=
[
x
k
−
1
^
v
k
y
k
]
H
=
[
1
−
A
k
−
1
1
C
k
]
W
=
[
P
k
−
1
^
Q
k
R
k
]
min(z_k-\check{z_k}) = minJ(x) = min \frac{1}{2}(z-Hx)^TW^{-1}(z-Hx)\\ z = \begin{bmatrix}\hat{x_{k-1}}\\v_k\\y_k \end{bmatrix} H = \begin{bmatrix} 1 \\ -A_{k-1} &1\\ &C_k\end{bmatrix}\\ W = \begin{bmatrix}\hat{P_{k-1}}\\&Q_k\\&&R_k\end{bmatrix}
min(zk−zkˇ)=minJ(x)=min21(z−Hx)TW−1(z−Hx)z=⎣⎡xk−1^vkyk⎦⎤H=⎣⎡1−Ak−11Ck⎦⎤W=⎣⎡Pk−1^QkRk⎦⎤
通过SMW的公式，推一推，换换变量就可以得到递推式（推导还是看书吧《State Estimation for Robotics》中文版 P56-57）：
整个系统的LLT求解
若将整个系统都纳入考量，系统中不止有一次测量，整合后写成提升形式并通过最大后验/贝叶斯推断将问题转换成一个典型的批量最小二乘问题，并采用LLT分解，同样可以得到卡尔曼滤波递推式，不同的是LLT分解还有个“反向传播”的修正过程，这将帮助整个系统得到在高斯模型下更精确的解（不过现实情况和高斯模型差多少，谁又知道呢？）。
J
(
x
)
=
1
2
(
z
−
H
x
)
T
W
−
1
(
z
−
H
x
)
J(x) = \frac{1}{2}(z-Hx)^TW^{-1}(z-Hx)\\
J(x)=21(z−Hx)TW−1(z−Hx)
H
=
[
1
−
A
0
1
⋱
⋱
−
A
k
−
1
1
C
0
C
1
⋱
C
k
]
W
=
[
P
0
ˇ
Q
1
Q
2
⋱
R
0
R
1
⋱
R
k
]
H = \begin{bmatrix} 1 \\ -A_0 &1\\ &\ddots & \ddots \\ &&-A_{k-1}&1\\C_0\\&C_1\\&&\ddots\\&&&C_k\end{bmatrix}\\ W = \begin{bmatrix}\check{P_0}\\&Q_1\\&&Q_2\\&&& \ddots \\&&&&R_0\\&&&&&R_1\\&&&&&&\ddots\\&&&&&&& R_{k} \end{bmatrix}
H=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡1−A0C01⋱C1⋱−Ak−1⋱1Ck⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤W=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡P0ˇQ1Q2⋱R0R1⋱Rk⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
最终结果
K k = P k ˇ C k T ( C k P k ˇ C k T + R k ) − 1 P k ^ = ( 1 − K k C k ) P k ˇ x k ^ = x k ˇ + K k ( y k − C k x k ˇ ) K_k = \check{P_k}C^T_k(C_k\check{P_k}C^T_k + R_k)^{-1}\\ \hat{P_k} = (1-K_kC_k)\check{P_k}\\ \hat{x_k} = \check{x_k} + K_k(y_k - C_k\check{x_k}) Kk=PkˇCkT(CkPkˇCkT+Rk)−1Pk^=(1−KkCk)Pkˇxk^=xkˇ+Kk(yk−Ckxkˇ)
总结：
书中给出三种推出卡尔曼滤波的方法：基于解析式的LLT分解、基于MAP和基于贝叶斯推断的三种方法，LLT更加全面，贝叶斯利用高斯模型在线性模型下的性质，直接求均值和方差得结果，而Map则可以看成LLT分解的马尔可夫版本。
参考文献：
1.《State Estimation for Robotics》
2.贺博博客
扫一扫
1320
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
