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

扩展卡尔曼滤波(EKF)算法详细推导及仿真(Matlab),第1张

姓名:王柯祎

学号:20021110373T

转自 :https://blog.csdn.net/gangdanerya/article/details/105105611

【嵌牛导读】介绍扩展卡尔曼滤波(EKF)梁返让算法的详细推导,局限性和MATLAB仿真。

【嵌牛鼻子】扩展卡尔曼滤波(EKF)

【嵌牛正文】

扩展卡尔曼滤波算法 是解决非线性状态估计问题最为直接的一种处理方法,尽管EKF不是最精确的”最优“滤波器,但在过去的几十年成功地应用到许多非线性系统中。所以在学习非线性滤波问题时应该先从EKF开始。

EKF算法是将非线性函数进行泰勒展开,然后省略高阶项,保留展开项的一阶项,以此来实现非线性函数线性化,最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值。

一、EKF算法详细推导

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

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

二、EKF算法局橡局限性:

该算法线性化会引世厅入阶段误差从而导致滤波精度下降,同时当初始状态误差较大或系统模型非线性程度较高时,滤波精度会受到严重影响甚至发散。

需要计算雅克比矩阵,复杂,计算量大,影响系统的实时性,还会导致EKF算法的数值稳定性差。

当系统存在模型失配,量测干扰,量测丢失,量测延迟或状态突变等复杂情况时,EKF算法鲁棒性差。

三、Matlab仿真:

clear allclc  close all

tf = 50 

Q = 10w=sqrt(Q)*randn(1,tf) 

R = 1v=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估计值')

仿真结果:

%% 连续空间系统状态方升简桐程

% X_dot(t) = A(t)*X(t) + B(t)*W(t)

%% 转化为离散时间系统状态方程

% X(k+1) = F*X(k) + G*W(k)

T = 0.1% T为采样吵坦咐森时间间隔

[F,G]=c2d(A,B,T)


欢迎分享,转载请注明来源:内存溢出

原文地址: http://outofmemory.cn/yw/12486501.html

(0)
打赏 微信扫一扫 微信扫一扫 支付宝扫一扫 支付宝扫一扫
上一篇 2023-05-25
下一篇 2023-05-25

发表评论

登录后才能评论

评论列表(0条)

保存