扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）

前言

扩展卡尔曼滤波算法是解决非线性状态估计问题最为直接的一种处理方法，尽管EKF不是最精确的”最优“滤波器，但在过去的几十年成功地应用到许多非线性系统中。所以在学习非线性滤波问题时应该先从EKF开始。
EKF算法是将非线性函数进行泰勒展开，然后省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化，最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值。

一、EKF算法详细推导

【注】EKF推导参考的是黄蔚的博士论文“CKF及鲁棒滤波在飞行器姿态估计中的应用研究”，论文中EKF，UKF和CKF等算法讲解的都很详细，值得一看。

 

 

 我们把KF与EKF算法拿出来对比可以发现：

 

EKF算法局限性：

•     该算法线性化会引入阶段误差从而导致滤波精度下降，同时当初始状态误差较大或系统模型非线性程度较高时，滤波精度会受到严重影响甚至发散。
•     需要计算雅克比矩阵，复杂，计算量大，影响系统的实时性，还会导致EKF算法的数值稳定性差。
•     当系统存在模型失配，量测干扰，量测丢失，量测延迟或状态突变等复杂情况时，EKF算法鲁棒性差。

 Matlab仿真：

clear all;
clc; 
close all;
tf = 50; 
Q = 10;w=sqrt(Q)*randn(1,tf); 
R = 1;v=sqrt(R)*randn(1,tf);
P =eye(1);
x=zeros(1,tf);
Xnew=zeros(1,tf);
x(1,1)=0.1; 
Xnew(1,1)=x(1,1);
z=zeros(1,tf);
z(1)=x(1,1)^2/20+v(1); 
zjian=zeros(1,tf);
zjian(1,1)=z(1);
for k = 2 : tf 
    %%%%%%%%%%%%%%%%%%模拟系统%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1);  
    z(k) = x(:,k).^2 / 20 + v(k); 
    %%%%%%%%%%%%%%%%%%EKF开始%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Xpre = 0.5*Xnew(:,k-1)+ 2.5*Xnew(:,k-1)/(1+Xnew(:,k-1).^2) + 8 * cos(1.2*(k-1));
    zjian =Xpre.^2/20; 
    F = 0.5 + 2.5 * (1-Xnew.^2)/((1+Xnew.^2).^2);
    H = Xpre/10;
    PP=F*P*F'+Q;
    Kk=PP*H'*inv(H*PP*H'+R);
    Xnew(k)=Xpre+Kk*(z(k)-zjian);
    P=PP-Kk*H*PP;
end
   t = 2 : tf;
   figure;
   plot(t,x(1,t),'b',t,Xnew(1,t),'r*');
   legend('真实值','EKF估计值');

转载：

扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）_gangdanerya的博客-CSDN博客_扩展卡尔曼滤波matlab