扩展卡尔曼滤波EKF  -- (更新09/9/4)

之前的我们介绍过卡尔曼滤波的一维和多维的基础知识。传送门

今天我们来看看什么是EKF。在了解EKF具体算法之前,先要知道为什么需要EKF。

1.非线性问题:

我们都知道卡尔曼滤波算法是让预测和更新都建立在线性计算基础上的,比如一个小车我们知道状态是{x,v}。我们预测在下一秒的位置是 [公式] 然后直接套用一维或多维公式就可以了。但是在现实世界中,往往都是非线性的。比如小车在环岛移动,或者是不停的走S型。这时候就会导致卡尔曼算法不法有效快速的进行计算。

2.状态分布:

并且状态都是呈现单峰的高斯分布的

这两个条件限制了卡尔曼滤波只能用在很有限的环境中。怎么解决他呢,这就是EKF的作用了。

我们先来看下为什么不能用卡尔曼滤波在非线性的环境中:

如上图所示一个单峰的高斯分布在经过线性计计算后,还是一个单峰的。

如果是一个非线性计算的话,结构明显就不是单峰的分布了。

所以我们需要模拟近似的从这个非线性算法中抽出线性的算法来近似计算。这样就可以让我们的卡尔曼滤波算法用于非线性环境。

有很多线性化非线性函数的技术,EKF主要使用的是一阶泰勒级数(Taylor Expansion).通过获取非线性函数在mean的斜率来构造近似线性函数。如下图:

使用泰勒级数近似非线性函数:

这是个无限的公式,我们只需要第一项导数就可以比较准确的近似了。所以缩减为:

通过近似后的函数进行卡尔曼算法,就能得到单峰的高斯分布。因为是近似所以一定是存在误差的,但是考虑到计算速度和简单性,进行近似还是很有必要的。

EKF预测公式:

g和h都是非线性函数,一个是运动模型位置后验,一个是测量数据后验,我们运用泰勒级数分别近似:

[公式]

[公式]

G也叫做jacobian,这个值和Ut有关,所以是一个不断变化的值。有了g函数的线性近似表示我们就可以求的t时刻的后验概率:

[公式]

同理我们测量后验概率也可以用一样的方式求:

[公式]

获取了线性近似之后,计算测量概率:

[公式]

在卡尔曼算法中我们注意到:

协方差P是通过F计算的,F是线性的。但是在EKF中,X'我们可以通过非线性函数进行预测,但是协方差P不行,应为会导致不是单峰的高斯分布。所以我们需要用近似的函数F进行P的计算。

H就是

H

其他和卡尔曼类似: