参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》这本书对kalman算法的解析很清晰，MATLAB程序很全，适合初学者（如有侵权，请联系删除(qq:1491967912)）
EKF滤波算法是建立在KF滤波算法的基础上，核心思想是，对于非线性系统，首先对滤波值
的非线性函数
展开成泰勒级数但只保存一阶及以下部分（舍去二阶和高阶部分），得到近似的线性化模型。然后就是利用KF算法完成对目标的滤波估计等处理。
EKF的优点是不需提前计算标准轨迹（过程噪声W（k）与观测噪声V（k）为0时的非线性方程的解），但它的缺陷是只能在滤波误差
和一步预测误差
较小的时候才能使用。
1、扩展kalman滤波原理
1.1 局部线性化
离散非线性系统动态方程可写为：
其中，当过程噪声W（k）和观测噪声V(k)恒为零时，模型1、2的解为非线性模型的理论解，称为“标称轨迹”或“标称状态”。另外，把非线性系统1、2的真实解称为“真轨迹”或“真实值”。
接下来是具体的数学推导。首先假定没有控制量的输入，过程噪声是均值为零的高斯白噪声，同时噪声的驱动矩阵G（k）是已知的，观测噪声V（k）是加性均值为零的高斯白噪声，并假定过程噪声W（k）和观测噪声V（k）二者之间相互独立。
首先进行的局部线性化的步骤，将EKF中的非线性模型局部线性化。这里会用到泰勒级数展开公式，关于泰勒级数的知识请参考如何通俗地解释泰勒公式？这里只给出泰勒公式的在0点的n阶展开公式：
根据系统状态方程1，将非线性函数f（*）围绕滤波值
做一阶泰勒展开，结果为：
令
将上面两个公式带入到公式3中得到线性化的状态方程为：
状态初始值为X(0)=E[X(0)]。与KF算法相比，在已经求得前一步滤波值的条件下
，状态方程4增加了非随机的外作用项
。
另外由系统量测方程，将非线性函数h(*)围绕滤波值，做一阶泰勒展开。得
令：
则观测方程为：
1.2、线性Kalman滤波
这样就将非线性化模型1、2转换为了线性化模型4、5，对这个线性化的模型应用kalman滤波基本方程可得扩展kalman滤波方程。
式中，滤波初值和滤波误差方差矩阵初值分别为：
与kalman滤波基本方程相比，在线性化后的系统方程中，状态转移矩阵F和观测矩阵H由f和h的雅可比矩阵替代。假设状态变量有n维，即
，则相应雅可比矩阵的求法如下。
1.3、扩展Kalman滤波器九大步骤：
第一步：初始化初始状态方程X(0)、观测方程Z（0）、协方差矩阵P0；
第二步：状态预测：
第三步：观测预测：
第四步：一阶线性化状态方程，求解状态转移矩阵F。
第五步：一阶线性化观测方程，求解观测矩阵H。
第六步：求协方差矩阵预测
。
第七步：求Kalman滤波增益
第八步：求状态更新
或者
第九步：协方差更新
上面的九个步骤就是EKF设计的一个计算周期，各个时刻EKF对非线性系统的处理就是这个计算周期不断循环的过程。
以上就是关于扩展Kalman滤波的全部原理和公式如果有错误还请批评指正。下一篇会通过一个程序对EKF进行分析和学习。继续加油。
标签：噪声,非线性,Kalman,EKF,基本知识,滤波,矩阵,线性化 From： https://www.cnblogs.com/sbb-first-blog/p/16608216.html