线性卡尔曼滤波的算法采用递推的形式表示，其算法已被学者们研究得较为透彻。但是实际应用中非线性的现象是十分普遍的，例如:物理模型受到外部干扰、存在非线性性和病态方差矩阵等因素所引起的非线性问题都使得传统的KF算法以不再适用，迫切需要改进KF算法得到适用于非线性系统的卡尔曼滤波技术。假设所有的变换都是准线性的，先对连续的非线性方程进行线性化和离散化处理，利用一阶泰勒展开式来近似非线性模型，得到对应于非线性系统的推广的卡尔曼滤波(EKF)算法。EKF围绕上一步估计值将非线性的状态方程及观测方程展成泰勒级数，并取一阶近似得到线性化模型，从而沿用标准KF递推体系。当系统为弱非线性时，EKF滤波精度较高，但当系统为强非线性时，EKF滤波精度大为降低甚至可能导致滤波发散。

非线性系统的状态方程为：

非线性系统的测量方程为：

其中 表示系统状态向量， 是测量输出，假设 和 对每一时刻的 都连续可微， 是系统干扰， 是测量噪声。t表示离散时刻下标。 和 是不相关的均值为0的高斯随机向量，协方差分别为 Q≥0，R>0，假设下列初始条件成立：

这里E(x)表示对随机变量x求期望， 为x的期望。

定义 和 分别为 和 的雅可比矩阵，记作：

EKF滤波算法可由两部分组成：

第一步：时间更新。

第二步：测量更新。

注：EKF通过在每一时刻对非线性模型进行一阶近似，忽略二阶及其高阶项，这势必会引入模型误差，因此EKF并不是最优滤波器。所以矩阵 和 并不代表真实状态估计的误差协方差矩阵，而且 和 依赖于前一时刻状态估计值，所以增益矩阵 、误差协方差矩阵 和 都不能像标准的卡尔曼滤波器那样离线计算。

代码实现（MATLAB）：

基于距离的目标跟踪算法

卡尔曼滤波器介绍：

卡尔曼滤波器知识整理

UKF无迹卡尔曼滤波器介绍：

无迹卡尔曼滤波器（UKF）

如果觉得《扩展卡尔曼滤波器（EKF）》对你有帮助，请点赞、收藏，并留下你的观点哦！