通俗易懂!视觉slam第十一部分——线性系统和卡尔曼滤波

首先来看滤波器模型。当我们假设了马尔可夫性,从数学角度会发生哪些变化呢?首先,当前时刻状态只和上一个时刻有关,上部分等式右侧第一部分可进一步简化:

这里,由于 k 时刻状态与 k − 1 之前的无关,所以就简化成只与x_{k-1}u_{k}有关的形式,与 k 时刻的运动方程对应。第二部分可简化为: 

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

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

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

为了简洁省略了 R 和 Q 的下标。现在,利用马尔可夫性,假设我们知道了 k − 1时刻的后验(在 k − 1 时刻看来)状态估计: 和它的协方差,现在要根据 k 时刻的输入和观测数据,确定 x_{k}的后验分布。为区分推导中的先验和后验,我们在记号上作一点区别:以尖帽子表示后验,以横线表示先验分布,请不要混淆。 

卡尔曼滤波器的第一步,通过运动方程确定x_{k}的先验分布。根据高斯分布的性质,显然有:

 这一步称为预测,它显示了如何从上一个时刻的状态,根据输入信息(但是有噪声),推断当前时刻的状态分布。这个分布也就是先验。记这里的:

这非常自然。另一方面,由观测方程,我们可以计算在某个状态下,应该产生怎样的观测数据: 

为了得到后验概率,我们想要计算它们的乘积,也就是由式(10.5)给出的贝叶斯公式。然而,虽然我们知道最后会得到一个关于x_{k}的高斯分布,但计算上是有一丁点儿麻烦的,我们先把结果设为x_{k} ∼ N(ˆ x k ,P k ),那么:

稍微用点讨巧的方法。既然我们已经知道等式两侧都是高斯分布,那就只需比较指数部分即可,而无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的配方,我们来推导一下。首先把指数部分展开,有  : 

为了求左侧的 ,我们把两边展开,并比较x_{k}的二次和一次系数。对于二次系数,有: 

该式给出了协方差的计算过程。为了便于后边列写式子,定义一个中间变量:

根据此定义,在左右各乘,有: 

于是有: 

然后再比较一次项的系数,有:

整理(取系数并转置)得:

两侧乘以并代入 得:

于是我们又得到了后验均值的表达。总而言之,上面的两个步骤可以归纳为“预测”(Predict)和“更新”(Update)两个步骤:

至此,我们推导了经典的卡尔曼滤波器的整个过程。事实上卡尔曼滤波器有若干种推导方式,而我们使用的是从概率角度出发的最大后验概率估计的形式。我们看到,在线性高斯系统中,卡尔曼滤波器构成了该系统中的最大后验概率估计。而且,由于高斯分布经过线性变换后仍服从高斯分布,所以整个过程中我们没有进行任何的近似。可以说,卡尔曼滤波器构成了线性系统的最优无偏估计。 

发布了370 篇原创文章 · 获赞 190 · 访问量 51万+

猜你喜欢

转载自blog.csdn.net/qq_32146369/article/details/104435340
今日推荐