SLAM -- 通过毫米波雷达详解EKF始末(2)

在非线性运动的状态估计中,会有两点影响我们对贝叶斯滤波的使用:

1.我们一般对于运动或测量的噪声都使用的是简单的加性噪声

[公式]

[公式]

但是在非线性情况下,往往也会对噪声造成影响,通常是这样的

[公式]

[公式]

2.我们通常所处理的那些高斯分布的噪声或状态值的分布会在非线性的环境下,也变成非高斯的,如下图:

这样就不在符合贝叶斯滤波的条件了,自然也就无法使用贝叶斯滤波估计状态了。解决办法就是我们通过近似来使非线性变成近似的线性。

常用的线性化工具是什么? 泰勒级数。

熟练掌握EKF的KEY在这:

我们需要将非线性预测和观测方程线性化,即进行一阶泰勒级数展开

一阶导数的形式如下:

接下去就是推导了,详细的过程机器人学中说的很具体了,可以去参考,这里简单的说就是通过一顿操作得出了贝叶斯滤波公式中子项们的高斯分布表示,随后方程两边左右比较得出最后的EKF公式:

最后得出的EKF公式如下,使用中直接套公式就可以很简单的实现了: