Ⅰ 擴展卡爾曼濾波（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 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估計值');
模擬結果：