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

姓名：王柯祎

学号：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估计值')

仿真结果：

%卡尔曼滤波

clear

N=800

w(1)=0

w=randn(1,N)

%系统预测的随机白噪声

x(1)=0

a=1

for

k=2:N

x(k)=a*x(k-1)+w(k-1)

%系统的预测值

end

V=randn(1,N)

%测量值的随机白噪声

q1=std(V)

Rvv=q1.^2

q2=std(x)

Rxx=q2.^2

q3=std(w)

Rww=q3.^2

c=0.2

Y=c*x+V

%测量值

p(1)=0

s(1)=0

for

t=2:N

p1(t)=a.^2*p(t-1)+Rww

%前一时刻X的相关系数

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)

%卡尔曼增益

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))

%经过滤波后的信号

p(t)=p1(t)-c*b(t)*p1(t)%t状态下x(t|t)的相关系数

end

figure(1)

plot(x)

title('系统的预测值')

figure(2)

plot(Y)

title('测量值')

figure(3)

plot(s)

title('滤波后的信号')

%这个问题我已经回答过了，下面是我以前的回复

clear

N=200%取200个数

w(1)=0

w=randn(1,N)%产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）

x(1)=0%状态x初始值

a=1%a为状态转移阵，此程序简单起见取1

for k=2:N

x(k)=a*x(k-1)+w(k-1) %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）

end

V=randn(1,N)%测量噪声

q1=std(V)

Rvv=q1.^2

q2=std(x)

Rxx=q2.^2%此方程未用到Rxx

q3=std(w)

Rww=q3.^2%Rvv、Rww分别为过程噪声和测量噪声的协方差(此方程只取一组数方差与协方差相同)

c=0.2

Y=c*x+V%量测方差，c为量测矩阵，同a简化取为一个数

p(1)=0%初始最优化估计协方差

s(1)=0%s(1)表示为初始最优化估计

for t=2:N

p1(t)=a.^2*p(t-1)+Rww%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)

p(t)=p1(t)-c*b(t)*p1(t)%此式由一步估计的协方差得到此时刻最优化估计的协方差

end

t=1:N

plot(t,s,'r',t,Y,'g',t,x,'b')%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态

%整体来说，此卡尔曼程序就是一个循环迭代的过程，给出初始的状态x和协方差p，得到下一时刻的x和p，循环带入可得到一系列的最优的状态估计值，此方法通常用于目标跟踪和定位。

%本人研究方向与此有关，有兴趣可以交流下

---