自动驾驶（十二）---------扩展卡尔曼滤波

       扩展卡尔曼滤波的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波。

       一般来说，EKF在对非线性函数做泰勒展开时，只取到一阶导和二阶导，而由于二阶导的计算复杂性，更多的实际应用只取到一阶导。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

与标准卡尔曼滤波对比：

1状态转移方程：

       标准卡尔曼滤波：

       扩展卡尔曼滤波：

       利用泰勒展开式在上次估值处展开：

                     

2 观测方程:

        标准卡尔曼滤波：

        扩展卡尔曼滤波：

        在状态预测值处展开

                              

         其中，和分别表示函数和在和处的雅克比矩阵。

3 EKF的传播公式：

                                           

4 EKF的更新公式：

                                              

其中的雅克比矩阵和分别为