卡爾曼濾波最新的改進濾波演算法

A. 擴展卡爾曼濾波（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估計值');

模擬結果：

B. 卡爾曼濾波本質是一種濾波方式還有什麼其他方法和卡爾曼濾波一樣的方法

卡爾曼濾波的一個典型實例是從一組有限的，對物體位置的，包含雜訊的觀察序列預測出物體的坐標位置及速度. 在很多工程應用(雷達, 計算機視覺)中都可以找到它的身影. 同時，卡爾曼濾波也是控制理論以及控制系統工程中的一個重要話題.狀態估計是卡爾曼濾波的重要組成部分。一般來說，根據觀測數據對隨機量進行定量推斷就是估計問題，特別是對動態行為的狀態估計，它能實現實時運行狀態的估計和預測功能。比如對飛行器狀態估計。狀態估計對於了解和控制一個系統具有重要意義，所應用的方法屬於統計學中的估計理論。最常用的是最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。其他如風險准則的貝葉斯估計、最大似然估計、隨機逼近等方法也都有應用。
受雜訊干擾的狀態量是個隨機量，不可能測得精確值，但可對它進行一系列觀測，並依據一組觀測值，按某種統計觀點對它進行估計。使估計值盡可能准確地接近真實值，這就是最優估計。真實值與估計值之差稱為估計誤差。若估計值的數學期望與真實值相等，這種估計稱為無偏估計。卡爾曼提出的遞推最優估計理論，採用狀態空間描述法，在演算法採用遞推形式，卡爾曼濾波能處理多維和非平穩的隨機過程。
卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在工程上得到了廣泛的應用，尤其在控制、制導、導航、通訊等現代工程方面。

C. 什麼叫卡爾曼濾波演算法其序貫演算法

卡爾曼濾波演算法（Kalman filtering）一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的演算法。由於觀測數據中包括系統中的雜訊和干擾的影響，所以最優估計也可看作是濾波過程。
序貫演算法又叫序貫相似性檢測演算法，是指圖像匹配技術是根據已知的圖像模塊(模板圖)在另一幅圖像(搜索圖)中尋找相應或相近模塊的過程，它是計算機視覺和模式識別中的基本手段。已在衛星遙感、空間飛行器的自動導航、機器人視覺、氣象雲圖分析及醫學x射線圖片處理等許多領域中得到了廣泛的應用。研究表明，圖像匹配的速度主要取決於匹配演算法的搜索策略。
數據濾波是去除雜訊還原真實數據的一種數據處理技術, Kalman濾波在測量方差已知的情況下能夠從一系列存在測量雜訊的數據中，估計動態系統的狀態. 由於, 它便於計算機編程實現, 並能夠對現場採集的數據進行實時的更新和處理, Kalman濾波是目前應用最為廣泛的濾波方法, 在通信, 導航, 制導與控制等多領域得到了較好的應用。

D. kalman濾波有哪些改進辦法

擴展Kalman濾波，無際Kalman濾波，集合kalman濾波，自適應kalman濾波，強跟蹤Kalman濾波

E. 哪本書講卡爾曼濾波講得好

最經典的當屬「 西北工大出版社出版的，秦永元的《卡爾曼濾波與組合導航原理》」

是想研究卡爾曼濾波的入門教材。

F. 卡爾曼濾波理解與實現

本文為離散卡爾曼濾波演算法的一 一個簡明教程，從演算法思想、實現過程、理論推導和程序實現四個方面闡述和分析了卡爾曼濾波演算法。

XU Ruilin完成本教程主要部分的編寫，WANG Xuejun完成第3節的編寫，ZHU Ximin完成2.2節的編寫，WEN Shuhan完成2.3節的編寫，MAO Bo完成全文整理、修訂和排版。

卡爾曼濾波（Kalman Filtering）及其一系列的優化和改進演算法是目前在求解運動狀態推算問題上最為普遍和高效的方法。 魯道夫·卡爾曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心訪問時，發現他的方法適用於解決阿波羅計劃的軌跡預測問題。阿波羅飛船的導航電腦就是使用這種濾波器進行軌跡預測。

卡爾曼濾波尤其適用於動態系統，這種方法對於內存要求極低而運算速度快，且能夠保持較好的計算精度，這使得這種方法非常適合解決實時問題和應用於嵌入式系統，也就是說，卡爾曼濾波天然的適用於解決艦艇指控系統的航跡推算問題。在接下來的內容里，我們將逐步領會卡爾曼濾波的這些絕佳特點。

不過，現在我們先從復雜的艦艇航跡推算問題中解脫出來，從一個更加熟悉和簡單的問題中來理解這個濾波演算法的思想、過程和演算法。

假設有一輛無人車WALL-E，需要導引它從A點到達B點，共有兩種手段（ 圖1 ）：

顯然，兩種方法都有一定的誤差。如果單獨採用某一種方法進行定位，WALL-E在誤差的影響下將無法到達B點。因此，需要將兩種方法結合起來，得到一個更加精確的結果，這就是卡爾曼濾波要解決的問題。

卡爾曼濾波方法如何看待我們的問題呢？在探究這個問題之前，我們先對問題進行抽象，並用數學語言來描述我們的問題。

我們用矢量 來描述WALL-E的運動狀態，這個列矢量 包括位置矢量 和速度矢量 兩個分量。在WALL-E的問題上，我們現在不知道位置 和速度 的准確值，但是知道WALL-E的運動模型滿足 狀態方程 ，定位的方法，也即觀測WALL-E運動狀態的方法滿足 觀測方程 . 當然，我們也知道，這兩種方法都存在一定的誤差 ，那麼我們的問題就可以轉化為一個優化問題——

在這一優化問題中，目標函數是要使預測（估計）誤差最小，同時約束於估計方法 和 的條件下。在卡爾曼濾波中，我們的估計原則（也就是最小化估計誤差的原則）是 最小方差無偏估計 [1] ，我們將通過後面的過程分析來說明這一點。

在我們正式開始引入公式分析卡爾曼濾波問題之前，我們還必須解決一個問題------把連續的線性系統離散化，也就是將連續時域問題轉化為時間序列問題。當然，目前我們只討論線性系統的情況，關於非線性系統問題，我們有擴展卡爾曼濾波（Extended Kalman Filtering, EKF）和無跡卡爾曼濾波（Unscented Kalman Filtering, UKF）兩種方法來求解。

補充內容------連續線性時變系統的離散化
設連續線性時變系統的時域狀態方程為

若采樣周期為 ，則從時刻 到時刻 ，有

令 ， ，則離散化的狀態方程為

通過對線性系統的離散化處理，我們現在可以考慮每一個時刻WALL-E的運動狀態。接下來，我們將用 來表示在 時刻運動狀態的最優估計值；用 表示用 時刻對 時刻的狀態預測值；用 表示對 時刻綜合預測和觀測兩種方法的最優估計值。

在估計WALL-E位置的問題上，假定我們已經知道它是勻速直線運動，WALL-E身上還攜帶有一個GPS感測器可以提供它的位置信息，WALL-E在前進過程中可能會遇到一些情況，比如停止前進或是受到風的影響。

加入我們已知的是WALL-E上一個時刻的最佳估計狀態，即k-1時刻的位置和速度，要求的是下一時刻即k時刻的最佳估計狀態，即k時刻的位置和速度，我們可以發現有兩種方法可以得到它的k時刻的狀態：

一種是通過WALL-E設定程序計算得到下一秒的狀態，比如現在設定是勻速直線運動，那麼下一秒的速度應該是恆定不變的，而位置則是在上一秒位置的基礎上加上時間乘以速度即一秒內走過的路程，但是現實生活中並不是理想的，機器人會受到摩擦力、風力等的影響，當然也可能會有頑皮的小孩擋住他前進的道路，這些因素使得WALL-E在k時的真實狀態與我們計算得到的數據有所不同。

另一種是通過WALL-E所攜帶的GPS來確定它的位置，因為GPS是測量出的就是WALL-E的實時狀態，因此它比較准確。但是GPS測量k時刻的狀態有兩個問題，一是GPS只能測出WALL-E的位置，而測不出它的速度；二是GPS感測器測量的時候也會有儀器的誤差，只能說它是比較准確的，比較接近真實值的。

那麼接下來問題來了，我們如何得到k時刻WALL-E的真實狀態呢？

我們將第一種方法得到的狀態值稱為預測值，第二種方法得到的狀態值稱為測量值，對汽車的最佳估計就是將這兩部分信息結合起來，盡量的去逼近k時刻的真實值。

下面再深入一些思考，怎麼將這兩部分結合起來？

在初始時間k-1， 是WALL-E的最佳估計值，WALL-E其實可以是估計值附近的任何位置，並且這種不確定性由該概率密度函數描述。WALL-E最有可能在這個分布的平均值附近。在下一個時間，估計的不確定性增加，用一個更大的方差表示，這是因為在時間步驟k-1和k之間，WALL-E可能收到了風力的影響，或者腳可能向前滑了一點，因此，它可能已經行進了與模型預測的距離不同的距離。

WALL-E位置的另一個信息來源來自測量，方差表示誤差測量的不確定性，真正的位置同樣可以是平均值附近的任何位置。

預測值和測量值，對WALL-E的最佳估計是將這兩部分信息結合起來，將兩個概率函數相乘得到另一個高斯函數，該估計值的方差小於先前估計值，並且該概率密度函數的平均值為我們提供了WALL-E位置的最佳估計。

以下，我們將進行e的運算推導

設：

則有實際目標變數的表達式：

數學模型中目標變數的表達式：

實際模型中測量變數的表達式：

數學模型中測量變數的表達式：

將目標變數的實際值和估計值相減：

將上述方程帶入誤差e的表達式，我們可得出誤差e的解析解：

從推導結果中我們不難看出，估計值和實際值的誤差隨時間呈指數形式變化，當（F-KH）<1時，隨著時間的推移，會無限趨近於零，也就是意味著估計值和實際值相吻合。這就是為什麼卡爾曼濾波器可以完美預測出目標狀態值的原理。

在估計WALL-E位置的問題上，我們不知道位置 和速度 的准確值，但是我們可以給出一個估計區間（ 圖5.a ）。卡爾曼濾波假設所有的變數是隨機的且符合高斯分布（正態分布）。每個變數有一個均值 和一個方差 （ 圖5.b ）。而 圖5.c 則表示速度和位置是相關的。

假如我們已知上一個狀態的位置值，現在要預測下一個狀態的位置值。如果我們的速度值很高，我們移動的距離會遠一點。相反，如果速度慢，WALL-E不會走的很遠。這種關系在跟蹤系統狀態時很重要，它給了我們更多的信息：一個觀測值告訴我們另一個觀測值可能是什麼樣子。這就是卡爾曼濾波的目的------從所有不確定信息中提取有價值的信息。

根據數理統計知識，我們知道這種兩個觀測值（隨機變數）之間的關系可以通過一個協方差矩陣

描述（ 圖6 ）。

我們假設系統狀態的分布為 高斯分布（正態分布） ，所以在 時刻我們需要兩個信息：最佳預估值 及其協方差矩陣 （如式(2)所示）。

下一步，我們需要通過 時刻的狀態來預測 時刻的狀態。請注意，我們不知道狀態的准確值，但是我們的預測函數並不在乎，它僅僅是對 時刻所有可能值的范圍進行預測轉移，然後得出一個k時刻新值的范圍。在這個過程中，位置 和速度 的變化為

我們可以通過一個狀態轉移矩陣 來描述這個轉換關系

同理，我們更新協方差矩陣 為

到目前為止，我們考慮的都是勻速運動的情況，也就是系統沒有對WALL-E的運動狀態進行控制的情況。那麼，如果系統對WALL-E進行控制，例如發出一些指令啟動或者制動輪子，對這些額外的信息，我們可以通過一個向量 來描述這些信息，並將其添加到我們的預測方程里作為一個修正。假如我們通過發出的指令得到預期的加速度 ，運動狀態方程就更新為

引入矩陣表示為

式中 稱為控制矩陣， 稱為控制向量（例如加速度 ）。當然，如果沒有任何外界動力影響的系統，可以忽略這一部分。

我們增加另一個細節，假如我們的預測轉換矩陣不是100%准確呢，會發生什麼？如果狀態只會根據系統自身特性演變，那樣將不會有任何問題。如果所有外界作用力對系統的影響可以被計算得十分准確，那樣也不會有任何問題。但是如果有些外力我們無法預測，例如我們在跟蹤一個四軸飛行器，它會受到風力影響；或者在跟蹤一個輪式機器人，輪子可能會打滑，地面上的突起會使它減速。我們無法跟蹤這些因素，而這些不確定事件發生時，預測方程將會失靈。因此，我們將這些不確定性統一建模，在預測方程中增加一個不確定項。

通過這種方式，使得原始狀態中的每一個點可以都會預測轉換到一個范圍，而不是某個確定的點（ 圖7.a ）。 可以這樣描述------ 中的每個點移動到一個符合方差 的高斯分布里（ 圖7.b ）。換言之，我們把這些不確定因素描述為方差為 的高斯雜訊，並用 表示。這樣就會產生一個新的高斯分布，方差不同，但是均值相同（ 圖7.c ）。

通過對 的疊加擴展，得到完整的預測轉換方程為

新的預測轉換方程只是引入了已知的系統控制因素。新的不確定性可以通過之前的不確定性計算得到。到這里，我們得到了一個模糊的估計范圍------通過 和 描述的范圍。

我們之前的工作仍然是在使用運動模型一種方法來估計系統的狀態，現在，我們要把另一種方法，也就是觀測（本問題中為GPS定位）考慮進來，以進一步修正對運動狀態的估計（ 圖8 ）。

我們用矩陣 來描述觀測方法的作用，於是有

再加入觀測雜訊 ，觀測方程為

從控制論的角度出發，我們定義新息（也即觀測值與預測值的誤差）為

當然我們也知道，觀測本身也會存在誤差，比如本問題中的GPS定位精度僅有10m. 因此，我們用矩陣 來描述這種不確定性（ 圖10 及 圖11.a ）。

這時，我們新息的協方差為

現在我們需要把兩種方法得到的可能性融合起來（ 圖11.b ）。對於任何狀態，有兩個可能性：1. 感測器的觀測值更接近系統真實狀態；2. 模型推算的估計值更接近系統真實狀態。如果有兩個相互獨立的獲取系統狀態的方式，並且我們想知道兩者都准確的概率值，於是我們可以通過加權來解決更相信誰的問題（ 圖11.c ）。

我們現在知道，系統模型的狀態預測 與對系統的狀態觀測 服從高斯分布，把這個問題抽象一下就是——

根據我們的一個估計准則------ 最小方差估計 ，那麼這個問題可以轉化為優化問題求解

求導數（差分）得

則 ，從而

當維度高於一維時，我們用矩陣來描述，有

這里的 稱為 卡爾曼增益 （Kalman Gain），也就是我們在解決更信任哪種方法時的偏向程度。

如果我們從兩個獨立的維度估計系統狀態，那麼根據系統模型的預測為

通過感測器的觀測為

我們結合著兩種方法得到

由 可知，卡爾曼增益為

將 約去（ 中也含有 項），得

此時的卡爾曼增益實際為

我們最後再來驗證一下 估計的無偏性 ——

這里我們設 時刻的真值為 ，由於

由於 （ 從初值而來的無偏傳遞性 ）可知 ，即卡爾曼濾波滿足無偏估計准則。顯然，其中要求系統雜訊和觀測雜訊是不相關、零期望的白雜訊，且是線性系統，初始時刻的狀態估計是無偏的。當這些條件不能滿足時，卡爾曼濾波的估計結果是有偏的。

到這里，我們已經獲得了卡爾曼濾波的全部要素。我們可以把整個過程總結為3個基本假設

假設一 和 都是零均值高斯白雜訊，也即 ，

假設二 與 無關，也即

假設三 系統初值 的均值和方差已知，且 與 均不相關。

以及5個基本方程 方程一 狀態預測

方程二 協方差預測

方程三 卡爾曼增益

G. 卡爾曼濾波的基本原理和演算法

卡爾曼濾波的原理用幾何方法來解釋。這時，~X和~Z矩陣中的每個元素應看做向量空間中的一個向量而不再是一個單純的數。這個向量空間（統計測試空間）可以看成無窮多維的，每一個維對應一個可能的狀態。~X和~Z矩陣中的每個元素向量都是由所有可能的狀態按照各自出現的概率組合而成（在測量之前，~X和~Z 的實際值都是不可知的）。~X和~Z中的每個元素向量都應是0均值的，與自己的內積就是他們的協方差矩陣。無法給出~X和~Z中每個元素向量的具體表達，但通過協方差矩陣就可以知道所有元素向量的模長，以及相互之間的夾角（從內積計算）。
為了方便用幾何方法解釋，假設狀態變數X是一個1行1列的矩陣（即只有一個待測狀態量），而量測變數Z是一個2行1列的矩陣（即有兩個測量儀器，共同測量同一個狀態量X），也就是說，m=1，n=2。矩陣X中只有X[1]一項，矩陣Z中有Z[1]和Z[2]兩項。Kg此時應是一個1行2列的矩陣，兩個元素分別記作Kg1 和 Kg2 。H和V此時應是一個2行1列的矩陣。

參考資料：
http://blog.csdn.net/newthinker_wei/article/details/11768443

H. 無味卡爾曼濾波與擴展卡爾曼濾波的具體區別，以及演算法

EKF是對非線性系統模型（方程）進行的線性化近似，以利用KF演算法進行濾波估計。而UKF是對狀態的概率統計近似，即設計少量的σ點，由σ點經由非線性函數的傳播，計算出隨機向量一、二階統計特性的傳播，對於高斯雜訊的假設，UKF能夠達到三階估計精度，而EKF只能達到二階精度，但其演算法仍然是利用KF的演算法。
現在國內外的文獻大都是對UKF演算法的改進和應用進行論述，但對演算法的穩定性等沒有系統的論述。我了解得沈陽自動化所做的這方面的工作很多。

I. 卡爾曼濾波演算法是什麼

卡爾曼濾波是一個濾波演算法，應用非常廣泛，它是一種結合先驗經驗、測量更新的狀態估計演算法，卡爾曼濾波器是在估計線性系統狀態的過程中，以最小均方誤差為目的而推導出的幾個遞推數學等式。

卡爾曼過程中要用到的概念。即什麼是協方差，它有什麼含義，以及什麼叫最小均方誤差估計，什麼是多元高斯分布。如果對這些有了了解，可以跳過，直接到下面的分割線。

均方誤差：

它是"誤差"的平方的期望值（誤差就是每個估計值與真實值的差），也就是多個樣本的時候，均方誤差等於每個樣本的誤差平方再乘以該樣本出現的概率的和。

方差：

方差是描述隨機變數的離散程度，是變數離期望值的距離。

注意:

兩者概念上稍有差別，當你的樣本期望值就是真實值時，兩者又完全相同。最小均方誤差估計就是指估計參數時要使得估計出來的模型和真實值之間的誤差平方期望值最小。

J. 卡爾曼濾波演算法的發展歷史如何

全球定位系統（GPS）是新一代的精密衛星導航定位系統。由於其全球性、全天候以及連續實時三維定位等特點，在軍事和民用領域得到了廣泛的發展。近年來，隨著科學技術的發展，GPS導航和定位技術已向高精度、高動態的方向發展。但是由於GPS定位包含許多誤差源，尤其是測量隨機誤差和衛星的幾何位置誤差，使定位精度受到影響。利用傳統的方法很難消除。而GPS動態濾波是消除GPS定位隨機誤差的重要方法，即利用特定的濾波方法消除各種隨機誤差，從而提高GPS導航定位精度。 經典的最優濾波包括：Wiener濾波和Kalman濾波。由於Wiener濾波採用頻域法，作用受到限制；而Kalman濾波採用時域狀態空間法，適合於多變數系統和時變系統及非平穩隨機過程，且由於其遞推特點容易在計算機上實現，因此得到了廣泛的應用。為此，本文對Kalman濾波方法進行了深入的研究，並取得了一些成果。 本文首先概述了GPS的組成、應用及最新動態。在此基礎上介紹了GPS的導航定位原理，給出了衛星可見性演算法、選星演算法及定位演算法。然後介紹了卡爾曼濾波的基本原理，在此基礎上對動態用戶的飛行軌跡進行了模擬，對「singer」模型下的8狀態和11狀態卡爾曼濾波演算法進行了模擬分析，同時對「當前」統計模型下11狀態卡爾曼濾波演算法進行了模擬分析，並對濾波前後的定位精度進行了比較。在此基礎上，就如何提高濾波器的動態性能作者提出了改進演算法，即自適應卡爾曼濾波演算法、帶漸消因子的優化演算法及改進的優化演算法，並分別進行了模擬分析。最後作者將卡爾曼濾波演算法分別應用於GPS/DR和GPS/INS組合導航定位系統中，並分別對這兩種系統進行了建模和模擬分析，取得了較理想的結果。 本文的研究工作，對改進傳統的濾波方法有一定的參考和應用價值，並對卡爾曼濾波方法在提高GPS動態導航定位精度方面的應用起到積極的促進作用。