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

马尔科夫和卡尔曼滤波

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

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

马尔科夫性质

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

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

从视觉slam到卡尔曼滤波

SLAM 过程由运动方程和观测方程来描述。那么，假设在 $t=0$ 到$t=N$ 的时间内，我们有${\mathbf{x}}_0$ 到 ${\mathbf{x}}_N$那么多个位姿，并且有${\mathbf{y}}_1,\dots ,{\mathbf{y}}_M$ 那么多个路标。一般来说，视觉slam过程的运动和观测方程为：

${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k$
${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$

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

${\mathbf{x}}_k\triangleq \left\{{\mathbf{x}}_k,{\mathbf{y}}_1,\dots ,{\mathbf{y}}_m\right\}$

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

$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k\end{array}k=1,\dots ,N$

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

$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)$

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

下面我们来看如何对状态进行估计。按照 Bayes 法则，把 $z_k$ 与 $x_k$ 交换位置，有：

$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)$

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

$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=\int P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)P\left({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}$

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

卡尔曼滤波

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

$P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)$

这里，由于 k 时刻状态与 k − 1 之前的无关，所以就简化成只与 xk−1 和 uk 有关的
形式，与 k 时刻的运动方程对应。第二部分可简化为：

$P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)$

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

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

$\{\begin{array}{l}{\mathbf{x}}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k\\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}k=1,\dots ,N$

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

${\mathbf{w}}_k\sim N(0,\mathbf{R}),{\mathbf{v}}_k\sim N(0,\mathbf{Q})$

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

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

预测：

${\overline{\mathbf{x}}}_k={\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\overline{\mathbf{P}}}_k={\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^T+\mathbf{R}$

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

$\mathbf{K}={\overline{P}}_k{C}_k^T{\left({\mathbf{C}}_{\mathbf{k}}{\overline{\mathbf{P}}}_k{\mathbf{C}}_k^T+\mathbf{Q}\right)}^{-1}$

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

$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\overline{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-C_k{\overline{\mathbf{x}}}_k\right)\\ {\hat{\mathbf{P}}}_k& =\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\overline{\mathbf{P}}}_k\end{array}$

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