1 简介
扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。
EKF的基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。
2 算法介绍
2.1 泰勒级数展开
泰勒级数展开是将一个在处具有
阶导数的函数
，利用关于
的
次多项式逼近函数值的方法。
若函数在包含
的某个闭区间
上具有
阶导数，且在开区间
上具有
阶导数，则对闭区间
上的任意一点
，都有：
其中表示函数
在
处的
阶导数，等式右边成为泰勒展开式，剩余的
是泰勒展开式的余项，是
的高阶无穷小。
（著名的欧拉公式就是利用
，
和
的泰勒展开式得来的！）
当状态量和观测量是多维向量时，一维的泰勒展开就需要做拓展，具体形式如下：
其中，表示雅克比矩阵，
表示海塞矩阵，
表示高阶无穷小。
这里，观测向量为维，状态向量为
维。
一般来说，EKF在对非线性函数做泰勒展开时，只取到一阶导和二阶导，而由于二阶导的计算复杂性，更多的实际应用只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。
2.1 EKF
标准卡尔曼滤波KF的状态转移方程和观测方程为
扩展卡尔曼滤波EKF的状态转移方程和观测方程为
（1）
（2）
利用泰勒展开式对（1）式在上一次的估计值处展开得
（3）
再利用泰勒展开式对（2）式在本轮的状态预测值处展开得
（4）
其中，和
分别表示函数
和
在
和
处的雅克比矩阵。
（注：这里对泰勒展开式只保留到一阶导，二阶导数以上的都舍去，噪声假设均为加性高斯噪声）
基于以上的公式，给出EKF的传播（Propagation）和更新（Update）两个步骤：
Propagation：
Update：
其中的雅克比矩阵和
分别为
，
雅可比矩阵的计算，在MATLAB中可以利用对自变量加上一个eps（极小数），然后用因变量的变化量去除以eps即可得到雅可比矩阵的每一个元素值。
读者可能好奇？为什么扩展卡尔曼滤波EKF的传播和更新的形式会和标准卡尔曼滤波KF的形式一致呢？以下做一个简单的推导。
3 推导
先列出几个变量的表示、状态转移方程和观测方程：
真实值，预测值
，估计值
，观测值
，观测值的预测
，估计值与真实值之间的误差协方差矩阵
，求期望的符号
。
，
，
引入反馈： （5）
OK，可以开始推导了：
由公式（3）（4）得到以下两个等式，标为式（6）（7）
计算估计值与真实值之间的误差协方差矩阵，并把式子（4）（5）（7）代入，得到
其中表示真实值与与预测值之间的误差协方差矩阵。于是得到式（8）
因为的对角元即为真实值与估计值的误差的平方，矩阵的迹（用
表示）即为总误差的平方和，即
要让估计值更接近于真实值，就要使上面的迹尽可能的小，因此要取得合适的卡尔曼增益，使得迹得到最小，言外之意就是使得迹对
的偏导为0，即
这样就能算出合适的卡尔曼增益了，即
代回式（8）得到
接下来就差真实值与预测值之间的协方差矩阵的求值公式了
将以下两个等式代入到上式，
，
可以得到
有、
与观测噪声
是独立的，求期望等于零;；
表示观测噪声的协方差矩阵，用
表示。于是得到
其中的协方差矩阵的转置矩阵就是它本身。OK，大功告成，以上就完成了全部公式的推导了。
4 实际应用
现在我们假设在海上有一艘正在做匀速直线运动的船只，其相对于传感器的横纵坐标为为隐藏状态，无法直接获得，而传感器可以测量得到船只相对于传感器的距离和角度
，传感器采样的时间间隔为
，则：
状态向量，观测向量
状态转移方程和观测方程为：
那么雅克比矩阵为
这里给定距离传感器的噪声均值为，方差为
；角度传感器的噪声均值为0，方差为
（单位弧度）；
采样时间点为个；
船只的初始状态为，四个状态量的噪声的方差分别为
。仿真结果如下：
从仿真结果可以看出，EKF在这种情形下的滤波效果还是不错的，但是在实际应用中，对于船只运动的状态转移噪声的均值和协方差矩阵
，以及传感器的观测噪声的均值
和协方差矩阵
，往往都是未知的，有很多情况都只有观测值而已，这样的情形下，就有必要利用观测值对噪声的统计量参数做出适当的估计（学习）。
5 参数估计（参数学习）
利用EM算法和极大后验概率估计（MAP），对未知的噪声参数做出估计，再利用估计出的参数去递推卡尔曼滤波的解。本文对EM算法在卡尔曼滤波框架中的推导暂时先不给出，之后可能会补充，这里就先给出一种Adaptive-EKF算法的公式。
，
，
（1）E-Step
Propagation：
Update：
（2）M-Step
利用以上的Adaptive-EKF算法对船只的运动做滤波跟踪，得到的效果如下图所示：
相比于没有做参数估计的EKF滤波，可以看出，Adaptive-EKF在估计误差上要优于EKF滤波，而且，它并不需要指定状态转移噪声和观测噪声的参数，将更有利于在实际中的应用。
6 总结
EKF滤波通过泰勒展开公式，把非线性方程在局部线性化，使得高斯分布的变量在经过线性变换后仍然为高斯分布，这使得能继续把标准卡尔曼滤波KF的框架拿过来用，总体来说，EKF在函数的非线性不是很剧烈的情形下，能够具有很不错的滤波效果。但是EKF也有它的不足之处：其一，它必须求解非线性函数的Jacobi矩阵，对于模型复杂的系统，它比较复杂而且容易出错；其二，引入了线性化误差，对非线性强的系统，容易导致滤波结果下降。基于以上原因，为了提高滤波精度和效率，以满足特殊问题的需要，就必须寻找新的逼近方法，于是便有了粒子滤波PF和无迹卡尔曼滤波UKF，笔者将在接下来的博文中为读者解读。
7 参考文献
[1] 林鸿. 基于EM算法的随机动态系统建模[J]. 福建师大学报(自然科学版), 2011, 27(6):33-37.
[2] https://www.cnblogs.com/gaoxiang12/p/5560360.html.
[3] https://max.book118.com/html/2017/0502/103920556.shtm.
原创性声明：本文属于作者原创性文章，小弟码字辛苦，转载还请注明出处。谢谢~
如果有哪些地方表述的不够得体和清晰，有存在的任何问题，亦或者程序存在任何考虑不周和漏洞，欢迎评论和指正，谢谢各路大佬。
需要代码和有需要相关技术支持的可咨询QQ：297461921