状态估计之卡尔曼滤波

本文主要介绍如何使用滤波器(卡尔曼滤波器与扩展卡尔曼滤波器)进行状态估计。

状态估计问题

首先列出状态估计方程，由运动方程与观测方程组成

其中

•  是相机在k时刻的位姿，我们一般可以使用变换矩阵或者其李代数表示它。
•  是k时刻系统的输入（在slam中通常是运动传感器的读数）
•  是运动过程中的噪声
•  是指在处进行了一次观测得到的观测值
•  是观测过程中的噪声

贝叶斯法则

对于上述状态估计问题，我们想做的是估计出现在状态的分布

按照贝叶斯法则，我们可以有如下结论

其中左边是后验概率分布（k时刻状态已经发生），右边第一项是似然概率分布，第二项是先验概率分布（k时刻状态还没发生）。

---

卡尔曼滤波

要使用卡尔曼滤波方法对上述后验概率分布进行估计，我们首先需要两条假设

1. 上述系统满足马尔可夫性，当前状态只与前一状态有关
2. 系统是线性高斯系统，高斯分布经过线性变换仍然是高斯分布

满足上述假设的状态估计问题可以重写为

其中第一个为运动方程，第二个为状态方程，并且所有的状态与噪声均满足高斯分布。这里噪声满足如下零均值高斯分布

由于马尔科夫性，假设我们已经有了时刻状态的后验概率分布：均值 与协方差  ，现在需要做的是根据k时刻的输入与观测数据，确定的后验概率分布。

此处需要先声明，对于噪声的协方差R与Q，为了方便我们省略了他们的下标。而我们用^表示后验，横线表示先验。

卡尔曼滤波的第一步是依据运动方程确定xkxk的先验概率分布。如下

其中，利用线性高斯系统的性质，我们得到的先验分布也是高斯分布，并且均值与方差如下：

下一步，我们由观测方程得到似然分布

然后，由贝叶斯法则我们知道，后验概率分布与似然分布和先验概率分布的乘积成正比，即

因为上述三个分布都是高斯分布，上述等式两侧又成正比，所以我们无需关系高斯分布系数部分内容，只需要保证等式两侧指数部分相等的即可。即得到下式

为了保证上面等式成立，我们将两边展开，令的二次系数与一次系数两边相等。对于二次系数，我们得到

这个等式给出了分布的协方差的计算过程，此处令，并将上式左右同时乘以，有

于是，我们得到卡尔曼滤波中的更新公式

其中，的更新公式是

同样，由一次项系数相同可以得到

整理可得卡尔曼滤波中的更新公式

---

卡尔曼滤波步骤归纳

由上面推导，我们可以将卡尔曼滤波归纳成如下预测与更新两个步骤：

• 预测：

• 更新： 首先计算K（卡尔曼增益）

然后计算后验概率的分布

卡尔曼滤波是线性系统的最优无偏估计

---

扩展卡尔曼滤波

上面的卡尔曼滤波是基于线性系统的，而我们实际工程遇到的问题一般都是非线性的，比如说slam中的运动方程与观测方程一般都是非线性函数。而一个高斯分布，经过非线性变换后，往往不再是高斯分布了。为了将卡尔曼滤波器的结果扩展到非线性系统中，我们通常会取一定的近似，将一个非高斯分布近似成高斯分布。

在扩展卡尔曼滤波器(Extended Kalman Filter, EKF)中，我们一般在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统的方法进行推导.

经过线性化（一阶泰勒展开）之后，我们得到的运动方程如下

观测方程如下

此处，我们另两个方程中的两个偏导数为

然后，根据运动方程，我们得到的先验概率分布为

根据观测方程，我们得到的似然概率分布为

下面的推导与卡尔曼滤波相同，通过贝叶斯公式使得高斯分布中的指数部分相同。

---

扩展卡尔曼滤波步骤总结

• 预测

• 更新，先计算K（卡尔曼增益）

然后更新后验分布的均值与方差

EKF的评价

• 优点
  • 推导简单清楚，适用各种传感器形式
  • 易于做多传感器融合
• 缺点
  • 一阶马尔可夫性过于简单，不能充分利用所有的信息
  • 一阶泰勒展开线性化遇到非线性严重的模型会误差比较大，并且只能局部近似
  • 需要存储所有状态量的均值与协方差矩阵，存储量呈平方增长
  • 对outlier很敏感
  • 假设都是高斯分布，不是对所有情况都适用

还有一些其他的滤波方法，比如粒子滤波、IF（信息滤波器）、UKF（Unscented KF）等等。其中粒子滤波不是假设高斯分布。