扩展Kalman滤波（EKF）基本知识和相应的matlab仿真

参考内容：书籍《卡尔曼滤波原理及应用------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对非线性系统的处理就是这个计算周期不断循环的过程。

2、matlab仿真程序

　　假设总时间为50s，过程噪声方差Q=5；观测噪声方差R=2；系统的状态方程和观测方程为：

　　　　状态方程：X（k+1）=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k)+w(k)
　　　　观测方程：Z(k)=X(k)^2/20+v(k)　

　　其中w(k)、v(k)是对应的过程噪声和观测噪声。假设初值X（0）=0.1，协方差的初值为P=1;具体程序如下：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%函数功能：标量非线性扩展kalman滤波问题
%状态方程：X（k+1）=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k)+w(k)
%观测方程：Z(k)=X(k)^2/20+v(k)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
close all
T=50;                 %总时间
Q=5;                 %Q值的改变会得到不同的滤波结果
R=2;                  %测量噪声
%产生过程误差
w=sqrt(Q)*randn(1,T);
%产生测量噪声
v=sqrt(R)*randn(1,T);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%状态方程
X=zeros(1,T);
X(1)=0.1;                 %状态方程初始化
Z=zeros(1,T);
Z(1)=X(1)^2/20+v(1);      %测量方程初始化
for i=2:T
    X(i)=0.5*X(i)+2.5*X(i)/(1+X(i)^2)+8*cos(1.2*i)+w(i);
    Z(i)=X(i)^2/20+v(i);
end
Xekf=zeros(1,T);
Xekf(1)=X(1);
I=eye(1);
P=[1];
for i=2:T
   Xekf(i)=0.5*X(i)+2.5*X(i)/(1+X(i)^2)+8*cos(1.2*i);           %状态预测
   Z1(i)=X(i)^2/20;
   F=0.5+2.5*(1-Xekf(i))/(1+Xekf(i)^2)^2;                      %求解状态转移矩阵
   H=Xekf(i)^2/20;                                                %求解观测矩阵
   P1=F*P*F+Q;
   K=P1*H'/(H*P1*H'+R);
   X(i)=Xekf(i)+K*(Z(i)-Z1(i));<br>　　P=(I-K*H)*P1;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%%%%误差分析
Xstd=zeros(1,T);
for i=1:T
    Xstd(i)=abs(X(i)-Xekf(i));
end
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%画图
figure;
hold on;box on;
plot(X,'-ko','MarkerFace','g');
plot(Xekf,'-bs','MarkerFace','r');
legend('真实值','EKF滤波的估计值');
xlabel('时间\s');
ylabel('状态值');
%误差分析
figure;
hold on;box on;
plot(Xstd,'-ko','MarkerFace','g');
xlabel('时间\s');
ylabel('状态估计偏差');