姓名：王柯祎
學(xué)號(hào)：20021110373T
轉(zhuǎn)自 ：https://blog.csdn.net/gangdanerya/article/details/105105611
【嵌牛導(dǎo)讀】介紹擴(kuò)展卡爾曼濾波（EKF）算法的詳細(xì)推導(dǎo)挖藏，局限性和MATLAB仿真。
【嵌牛鼻子】擴(kuò)展卡爾曼濾波（EKF）
【嵌牛正文】
擴(kuò)展卡爾曼濾波算法是解決非線性狀態(tài)估計(jì)問題最為直接的一種處理方法腹泌，盡管EKF不是最精確的”最優(yōu)“濾波器乡恕，但在過去的幾十年成功地應(yīng)用到許多非線性系統(tǒng)中言询。所以在學(xué)習(xí)非線性濾波問題時(shí)應(yīng)該先從EKF開始。
EKF算法是將非線性函數(shù)進(jìn)行泰勒展開傲宜，然后省略高階項(xiàng)运杭，保留展開項(xiàng)的一階項(xiàng)，以此來實(shí)現(xiàn)非線性函數(shù)線性化蛋哭，最后通過卡爾曼濾波算法近似計(jì)算系統(tǒng)的狀態(tài)估計(jì)值和方差估計(jì)值县习。
一、EKF算法詳細(xì)推導(dǎo)
【注】EKF推導(dǎo)參考的是黃蔚的博士論文“CKF及魯棒濾波在飛行器姿態(tài)估計(jì)中的應(yīng)用研究”谆趾，論文中EKF躁愿，UKF和CKF等算法講解的都很詳細(xì)，值得一看沪蓬。
我們把KF與EKF算法拿出來對(duì)比可以發(fā)現(xiàn)：
二彤钟、EKF算法局限性：
該算法線性化會(huì)引入階段誤差從而導(dǎo)致濾波精度下降，同時(shí)當(dāng)初始狀態(tài)誤差較大或系統(tǒng)模型非線性程度較高時(shí)跷叉，濾波精度會(huì)受到嚴(yán)重影響甚至發(fā)散逸雹。
需要計(jì)算雅克比矩陣营搅，復(fù)雜，計(jì)算量大梆砸，影響系統(tǒng)的實(shí)時(shí)性转质，還會(huì)導(dǎo)致EKF算法的數(shù)值穩(wěn)定性差。
當(dāng)系統(tǒng)存在模型失配帖世，量測(cè)干擾休蟹，量測(cè)丟失，量測(cè)延遲或狀態(tài)突變等復(fù)雜情況時(shí)日矫，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
%%%%%%%%%%%%%%%模擬系統(tǒng)%%%%%%%%%%%%%%%
????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('真實(shí)值','EKF估計(jì)值');
仿真結(jié)果：
