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

学号:20021110373T

from:/gangdanerya/article/details/105105611。

介绍了扩展卡尔曼滤波(EKF)算法的详细推导、局限性和MATLAB仿真。

嵌入牛鼻的扩展卡尔曼滤波器(EKF)

镶嵌牛文字

扩展卡尔曼滤波算法是解决非线性状态估计问题最直接的方法。虽然EKF滤波器不是最精确的“最优”滤波器，但在过去的几十年里，它已经成功地应用于许多非线性系统。所以学习非线性滤波问题要从EKF开始。

EKF算法是通过非线性函数的泰勒展开将非线性函数线性化，然后省略高阶项，保留展开项的一阶项。最后，利用卡尔曼滤波算法近似计算系统的状态估计和方差估计。

1.EKF算法的详细推导。

注:EKF的推导参考了黄伟博士论文《CKF和鲁棒滤波在飞行器姿态估计中的应用研究》。文中的EKF、UKF、CKF算法都有详细讲解，值得一看。

我们比较了KF和e KF算法，发现:

二、EKF算法的局限性:

这种算法的线性化会引入阶段误差，导致滤波精度下降。同时，当初始状态误差较大或系统模型非线性时，滤波精度会受到严重影响甚至发散。

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

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

三、Matlab仿真:

全部清除；clc？全部关闭；

tf = 50？

q = 10；w=sqrt(Q)*randn(1，TF)；？

r = 1；v=sqrt(R)*randn(1，TF)；

p =眼睛(1)；

x =零(1，TF)；

Xnew=zeros(1，TF)；

x(1，1)= 0.1；？

Xnew(1，1)=x(1，1)；

z =零(1，TF)；

z(1)=x(1,1)^2/20+v(1)；

zjian=zeros(1，TF)；

zjian(1，1)= z(1)；

对于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；

结束

？t = 2:TF；

？图；plot(t，x(1，t)，' b '，t，Xnew(1，t)，' r * ')；？图例('真实值'，' EKF估计值')；

模拟结果: