扩展卡尔曼滤波python_扩展卡尔曼滤波EKF与多传感器融合

参考：https://blog.csdn.net/young_gy/article/details/78468153

Extended Kalman Filter(扩展卡尔曼滤波)是卡尔曼滤波的非线性版本。在状态转移方程确定的情况下，EKF已经成为了非线性系统状态估计的事实标准。本文将简要介绍EKF，并介绍其在无人驾驶多传感器融合上的应用。

KF与EKF

本文假定读者已熟悉KF，若不熟悉请参考卡尔曼滤波简介。

KF与EKF的区别如下：

预测未来：x′=Fx+u用x′=f(x,u)代替；其余F用Fj代替。

修正当下：将状态映射到测量的Hx′用h(x′)代替；其余H用Hj代替。

其中，非线性函数f(x,u)，h(x′)用非线性得到了更精准的状态预测值、映射后的测量值；线性变换Fj，，Hj通过线性变换使得变换后的x，z仍满足高斯分布的假设。

Fj，Hj计算方式如下：

为什么要用EKF

KF的假设之一就是高斯分布的x预测后仍服从高斯分布，高斯分布的x变换到测量空间后仍服从高斯分布。可是，假如F、H是非线性变换，那么上述条件则不成立。

将非线性系统线性化

既然非线性系统不行，那么很自然的解决思路就是将非线性系统线性化。

对于一维系统，采用泰勒一阶展开即可得到：