视觉slam-----第四篇 卡尔曼滤波

马尔科夫和卡尔曼滤波

《视觉slam十四讲》读书笔记。

很多人都疑惑，马尔科夫又是什么意思？一个滤波问题怎么才能用得上卡尔曼滤波呢？扩展卡尔曼滤波又和卡尔曼滤波有什么关系？本篇博客不讲这些内容之间的推导关系，毕竟已经有无数人详细讲解了这个过程，这里只大概捋一捋它们之间的关系，已经我们该如何进行选择。

马尔科夫性质

一个马尔科夫过程就是指过程中的每个状态的转移只依赖于之前的 n个状态，这个过程被称为1个 n阶的模型，其中 n是影响转移状态的数目。最简单的马尔科夫过程就是一阶过程，每一个状态的转移只依赖于其之前的那一个状态。

在卡尔曼滤波中，我们就会使用马尔科夫性质，而且是一阶过程，我们不用关心它的推导过程。在卡尔曼滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。

从视觉slam到卡尔曼滤波

SLAM 过程由运动方程和观测方程来描述。那么，假设在 t = 0 t = 0 t=0 到 t = N t = N t=N 的时间内，我们有 x 0 \boldsymbol { x } _ { 0 } x0​ 到 x N \boldsymbol { x } _ { N } xN​那么多个位姿，并且有 y 1 , … , y M \boldsymbol { y } _ { 1 } , \dots , \boldsymbol { y } _ { M } y1​,…,yM​ 那么多个路标。一般来说，视觉slam过程的运动和观测方程为：

x k = f ( x k − 1 , u k ) + w k \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } xk​=f(xk−1​,uk​)+wk​

z k , j = h ( y j , x k ) + v k , j \boldsymbol { z } _ { k , j } = h \left( \boldsymbol { y } _ { j } , \boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k , j } zk,j​=h(yj​,xk​)+vk,j​

之前已经介绍了最大似然估计，把状态估计转换为最小二乘的做法。首先，由于位姿和路标点都是待估计的变量，我们改变一下记号，令 x k \boldsymbol { x } _ { k } xk​为 k k k时刻的所有未知量。它包含了当前时刻的相机位姿与 m m m 个路标点。在这种记号的意义下（虽然与之前稍有不同，但含义是清楚的），写成：

x k ≜ { x k , y 1 , … , y m } \boldsymbol { x } _ { k } \triangleq \left\{ \boldsymbol { x } _ { k } , \boldsymbol { y } _ { 1 } , \dots , \boldsymbol { y } _ { m } \right\} xk​≜{xk​,y1​,…,ym​}

把 k k k时刻的所有观测记作 z k z _ { k } zk​。于是，运动方程与观测方程的形式可写得更加简洁。这里不会出现 y \boldsymbol { y } y，但我们要明白这时 x \boldsymbol { x } x 中已经包含了之前的 y \boldsymbol { y } y 了:

{ x k = f ( x k − 1 , u k ) + w k z k = h ( x k ) + v k k = 1 , … , N \left\{ \begin{array} { l l } { \boldsymbol { x } _ { k } = f \left( \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) + \boldsymbol { w } _ { k } } \\ { \boldsymbol { z } _ { k } = h \left( \boldsymbol { x } _ { k } \right) + \boldsymbol { v } _ { k } } \end{array} \right. k = 1 , \ldots , N {xk​=f(xk−1​,uk​)+wk​zk​=h(xk​)+vk​​k=1,…,N

现在考虑第 k \boldsymbol { k} k时刻的情况。我们希望用过去 0 到 k \boldsymbol { k} k中的数据，来估计现在的状态分布:

P ( x k ∣ x 0 , u 1 : k , z 1 : k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) P(xk​∣x0​,u1:k​,z1:k​)

下标 0 : k 0 : k 0:k 表示从 0 时刻到 k k k 时刻的所有数据。请注意 z k z _ { k } zk​ 来表达所有在 k k k时刻的观测数据，注意它可能不止一个，只是这种记法更加方便。

下面我们来看如何对状态进行估计。按照 Bayes 法则，把 z k z _ { k } zk​ 与 x k x_ { k } xk​ 交换位置，有：

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( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k } \right) \propto P \left( \boldsymbol { z } _ { k } | \boldsymbol { x } _ { k } \right) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) P(xk​∣x0​,u1:k​,z1:k​)∝P(zk​∣xk​)P(xk​∣x0​,u1:k​,z1:k−1​)

里第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，我们要明白当前状态 x k \boldsymbol { x } _ { k } xk​ 是基于过去所有的状态估计得来的。至少，它会受 x k − 1 \boldsymbol { x } _ { k - 1 } xk−1​ 影响，于是按照 x k − 1 \boldsymbol { x } _ { k - 1 } xk−1​时刻为条件概率展开：

P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) = ∫ P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1 P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = \int P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) P \left( \boldsymbol { x } _ { k - 1 } | \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) \mathrm { d } \boldsymbol { x } _ { k - 1 } P(xk​∣x0​,u1:k​,z1:k−1​)=∫P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)P(xk−1​∣x0​,u1:k​,z1:k−1​)dxk−1​

如果我们考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心 k k k 时刻和 k − 1 k - 1 k−1 时刻的情况。至此，我们给出了贝叶斯估计。接下来，我们根据一阶马尔科夫性，简单的一阶马氏性认为， k k k 时刻状态只与 k − 1 k - 1 k−1 时刻状态有关，而与再之前的无关。对于这种问题，我们可以用扩展卡尔曼滤波 EKF 来解决。

卡尔曼滤波

根据马尔可夫性，当前时刻状态只和上一个时刻有关，上式中等式右侧第一部分可进一步简化：

P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k ∣ x k − 1 , u k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)=P(xk​∣xk−1​,uk​)

这里，由于 k 时刻状态与 k − 1 之前的无关，所以就简化成只与 xk−1 和 uk 有关的

形式，与 k 时刻的运动方程对应。第二部分可简化为：

P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) = P ( x k ∣ x k − 1 , u k ) P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { x } _ { 0 } , \boldsymbol { u } _ { 1 : k } , \boldsymbol { z } _ { 1 : k - 1 } \right) = P \left( \boldsymbol { x } _ { k } | \boldsymbol { x } _ { k - 1 } , \boldsymbol { u } _ { k } \right) P(xk​∣xk−1​,x0​,u1:k​,z1:k−1​)=P(xk​∣xk−1​,uk​)

到k 时刻的输入量 u k \boldsymbol { u } _ { k } uk​ 与 k − 1 k-1 k−1 时刻的状态无关，所以我们把 u k \boldsymbol { u } _ { k } uk​ 拿掉。可以看到，这一项实际是 k − 1 k-1 k−1 时刻的状态分布。于是，这一系列方程说明了，我们实际在做的是“如何把 k − 1 k-1 k−1时刻的状态分布推导至 k k k 时刻”这样一件事。也就是说，在程序运行期间，我们只要维护一个状态量，对它进行不断地迭代和更新即可。进一步，如果假设状态量服从高斯分布，那我们只需考虑维护状态量的均值和协方差即可。

从形式最简单的线性高斯系统开始，最后会得到卡尔曼滤波器。线性高斯系统是说，运动方程和观测方程可以由线性方程来描述：

{ x k = A k x k − 1 + u k + w k z k = C k x k + v k k = 1 , … , N \left\{ \begin{array} { l l } { \boldsymbol { x } _ { k } = \boldsymbol { A } _ { k } \boldsymbol { x } _ { k - 1 } + \boldsymbol { u } _ { k } + \boldsymbol { w } _ { k } } \\ { \boldsymbol { z } _ { k } = \boldsymbol { C } _ { k } \boldsymbol { x } _ { k } + \boldsymbol { v } _ { k } } \end{array} \right. \quad k = 1 , \ldots , N {xk​=Ak​xk−1​+uk​+wk​zk​=Ck​xk​+vk​​k=1,…,N

并假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布：

w k ∼ N ( 0 , R ) , v k ∼ N ( 0 , Q ) \boldsymbol { w } _ { k } \sim N ( \mathbf { 0 } , \boldsymbol { R } ) , \quad \boldsymbol { v } _ { k } \sim N ( \mathbf { 0 } , \boldsymbol { Q } ) wk​∼N(0,R),vk​∼N(0,Q)

为了简洁我省略了 R R R和 Q Q Q 的下标。现在，利用马尔可夫性，假设我们知道了 k − 1 k-1 k−1时刻的后验（在 k − 1 k-1 k−1 时刻看来）状态估计 x ^ k − 1 \hat { \boldsymbol { x } } _ { k - 1 } x^k−1​和它的协方差 P ^ k − 1 \hat { \boldsymbol { P } } _ { k - 1 } P^k−1​，现在要根据 k k k 时刻的输入和观测数据，确定 x k x _ { k } xk​ 的后验分布。为区分推导中的先验和后验，我们在记号上作一点区别：以 x ^ k \hat { \boldsymbol { x } } _ { k } x^k​表示后验，以 x ‾ \overline { x } x 表示先验分布。

经典的卡尔曼滤波器主要分为两个过程，预测和更新。

预测：

x ‾ k = A k x ^ k − 1 + u k , P ‾ k = A k P ^ k − 1 A k T + R \overline { \boldsymbol { x } } _ { k } = \boldsymbol { A } _ { k } \hat { \boldsymbol { x } } _ { k - 1 } + \boldsymbol { u } _ { k } , \quad \overline { \boldsymbol { P } } _ { k } = \boldsymbol { A } _ { k } \hat { \boldsymbol { P } } _ { k - 1 } \boldsymbol { A } _ { k } ^ { T } + \boldsymbol { R } xk​=Ak​x^k−1​+uk​,Pk​=Ak​P^k−1​AkT​+R

更新：先计算 K K K，它又称为卡尔曼增益：

K = P ‾ k C k T ( C k P ‾ k C k T + Q ) − 1 \boldsymbol { K } = \overline { P } _ { k } C _ { k } ^ { T } \left( \boldsymbol { C _ { k } } \overline { \boldsymbol { P } } _ { k } \boldsymbol { C } _ { k } ^ { T } + \boldsymbol { Q } \right) ^ { - 1 } K=Pk​CkT​(Ck​Pk​CkT​+Q)−1

然后计算后验概率的分布：

x ^ k = x ‾ k + K ( z k − C k x ‾ k ) P ^ k = ( I − K C k ) P ‾ k \begin{aligned} \hat { \boldsymbol { x } } _ { k } & = \overline { \boldsymbol { x } } _ { k } + \boldsymbol { K } \left( \boldsymbol { z } _ { k } - C _ { k } \overline { \boldsymbol { x } } _ { k } \right) \\ \hat { \boldsymbol { P } } _ { k } & = \left( \boldsymbol { I } - \boldsymbol { K } \boldsymbol { C } _ { k } \right) \overline { \boldsymbol { P } } _ { k } \end{aligned} x^k​P^k​​=xk​+K(zk​−Ck​xk​)=(I−KCk​)Pk​​

卡尔曼滤波器构成了线性系统的最优无偏估计。