擴展卡爾曼濾波python（擴展卡爾曼濾波的優缺點）

• 1、卡爾曼濾波的詳細原理
• 2、卡爾曼濾波理解與實現
• 3、擴展卡爾曼濾波問題
• 4、卡爾曼濾波
• 5、卡爾曼濾波的應用

卡爾曼濾波（Kalman filtering）是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由於觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。

斯坦利·施密特(Stanley Schmidt)首次實現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦使用了這種濾波器。 關於這種濾波器的論文由Swerling (1958), Kalman (1960)與 Kalman and Bucy (1961)發表。

數據濾波是去除噪聲還原真實數據的一種數據處理技術, Kalman濾波在測量方差已知的情況下能夠從一系列存在測量噪聲的數據中，估計動態系統的狀態. 由於, 它便於計算機編程實現, 並能夠對現場採集的數據進行實時的更新和處理, Kalman濾波是目前應用最為廣泛的濾波方法, 在通信, 導航, 制導與控制等多領域得到了較好的應用.

表達式

X(k)=A X(k-1)+B U(k)+W(k)

背景

斯坦利·施密特(Stanley Schmidt)首次實

現了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問時,發現他的方法對於解決阿波羅計劃的軌道預測很有用,後來阿波羅飛船的導航電腦使用了這種濾波器。關於這種濾波器的論文由Swerling (1958), Kalman (1960)與 Kalman and Bucy (1961)發表。

定義

傳統的濾波方法，只能是在有用信號與噪聲具有不同頻帶的條件下才能實現．20世紀40年代，N．維納和A．H．柯爾莫哥羅夫把信號和噪聲的統計性質引進了濾波理論，在假設信號和噪聲都是平穩過程的條件下，利用最優化方法對信號真值進行估計，達到濾波目的，從而在概念上與傳統的濾波方法聯繫起來，被稱為維納濾波。這種方法要求信號和噪聲都必須是以平穩過程為條件。60年代初，卡爾曼(R．E．Kalman)和布塞(R． S．Bucy)發表了一篇重要的論文《線性濾波和預測 理論的新成果》，提出了一種新的線性濾波和預測理由論，被稱之為卡爾曼濾波。特點是在線性狀態空間表示的基礎上對有噪聲的輸入和觀測信號進行處理，求取系統狀態或真實信號。

這種理論是在時間域上來表述的，基本的概念是：在線性系統的狀態空間表示基礎上，從輸出和輸入觀測數據求系統狀態的最優估計。這裡所說的系統狀態，是總結系統所有過去的輸入和擾動對系統的作用的最小參數的集合，知道了系統的狀態就能夠與未來的輸入與系統的擾動一起確定系統的整個行為。

卡爾曼濾波不要求信號和噪聲都是平穩過程的假設條件。對於每個時刻的系統擾動和觀測誤差（即噪聲），只要對它們的統計性質作某些適當的假定，通過對含有噪聲的觀測信號進行處理，就能在平均的意義上，求得誤差為最小的真實信號的估計值。因此，自從卡爾曼濾波理論問世以來，在通信系統、電力系統、航空航天、環境污染控制、工業控制、雷達信號處理等許多部門都得到了應用，取得了許多成功應用的成果。例如在圖像處理方面，應用卡爾曼濾波對由於某些噪聲影響而造成模糊的圖像進行復原。在對噪聲作了某些統計性質的假定後，就可以用卡爾曼的算法以遞推的方式從模糊圖像中得到均方差最小的真實圖像，使模糊的圖像得到復原。

性質

①卡爾曼濾波是一個算法，它適用於線性、離散和有限維繫統。每一個有外部變量的自回歸移動平均系統(ARMAX)或可用有理傳遞函數表示的系統都可以轉換成用狀態空間表示的系統，從而能用卡爾曼濾波進行計算。

②任何一組觀測數據都無助於消除x(t)的確定性。增益K(t)也同樣地與觀測數據無關。

③當觀測數據和狀態聯合服從高斯分布時用卡爾曼遞歸公式計算得到的是高斯隨機變量的條件均值和條件方差，從而卡爾曼濾波公式給出了計算狀態的條件概率密度的更新過程線性最小方差估計，也就是最小方差估計。

形式

卡爾曼濾波已經有很多不同的實現，卡爾曼最初提出的形式一般稱為簡單卡爾曼濾波器。除此以外，還有施密特擴展濾波器、信息濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種。最常見的卡爾曼濾波器是鎖相環，它在收音機、計算機和幾乎任何視頻或通訊設備中廣泛存在。

實例

卡爾曼濾波的一個典型實例是從一組有限的，對物體位置的，包含噪聲的觀察序列中預測出物體的坐標位置及速度。在很多工程應用(雷達、計算機視覺)中都可以找到它的身影。同時，卡爾曼濾波也是控制理論以及控制系統工程中的一個重要話題。

應用

比如，在雷達中，人們感興趣的是跟蹤目標，但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響，得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波)，也可以是對於將來位置的估計(預測)，也可以是對過去位置的估計(插值或平滑)。

擴展卡爾曼濾波（EXTEND KALMAN FILTER, EKF）

是由kalman filter考慮時間非線性的動態系統，常應用於目標跟蹤系統。

狀態估計

狀態估計是卡爾曼濾波的重要組成部分。一般來說，根據觀測數據對隨機量進行定量推斷就是估計問題，特別是對動態行為的狀態估計，它能實現實時運行狀態的估計和預測功能。比如對飛行器狀態估計。狀態估計對於了解和控制一個系統具有重要意義，所應用的方法屬於統計學中的估計理論。最常用的是最小二乘估計，線性最小方差估計、最小方差估計、遞推最小二乘估計等。其他如風險準則的貝葉斯估計、最大似然估計、隨機逼近等方法也都有應用。

狀態量

受噪聲干擾的狀態量是個隨機量，不可能測得精確值，但可對它進行一系列觀測，並依據一組觀測值，按某種統計觀點對它進行估計。使估計值儘可能準確地接近真實值，這就是最優估計。真實值與估計值之差稱為估計誤差。若估計值的數學期望與真實值相等，這種估計稱為無偏估計。卡爾曼提出的遞推最優估計理論，採用狀態空間描述法，在算法採用遞推形式，卡爾曼濾波能處理多維和非平穩的隨機過程。

理論

卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在工程上得到了廣泛的應用，尤其在控制、制導、導航、通訊等現代工程方面。

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

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個基本方程 方程一 狀態預測

方程二 協方差預測

方程三 卡爾曼增益

擴展卡爾曼濾波本身是不受觀測值影響的，其結果總是收斂的，你肯定是程序有問題

將預測值和測量值進行結合，對系統狀態進行最優估計的算法。

在連續變化的系統中使用卡爾曼濾波是非常理想的，它具有佔用內存小的優點（除了前一個狀態量外，不需要保留其它歷史數據），並且速度很快，很適合應用於實時問題和嵌入式系統。

根據k-1時刻的系統狀態預測k時刻系統狀態。

考慮外部因素控制的影響

外部因素會對系統進行控制，從而帶來一些與系統自身狀態沒有相關性的改變。其中 成為控制矩陣， 稱為控制向量，如果沒有外部控制，這部分可以忽略。

外部噪聲因素

在每次預測之後，我們可以添加一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型

小結：

由上兩式可知，新的最優估計是根據上一最優估計預測得到的，並加上已知外部控制量的修正。 而新的不確定性由上一不確定性預測得到，並加上外部環境的干擾。

加入傳感器觀測數據

卡爾曼濾波的一大優點就是能處理傳感器噪聲，我們的傳感器或多或少都有點不可靠，並且原始估計中的每個狀態可以和一定範圍內的傳感器讀數對應起來。 從測量到的傳感器數據中，我們大致能猜到系統當前處於什麼狀態。但是由於存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。

傳感器早上用協方差 表示，該分布的均值 是我們讀取到的傳感器數據。

於是我們得到兩個高斯分布，一個是預測值附近，一個是傳感器讀數附近。把兩個具有不同均值和方差的高斯分布相乘，得到一個新的具有獨立均值和方差的高斯分布。

結果如下，其中，K為卡爾曼增益。

總結：

我們可以用這些公式對任何線性系統建立精確的模型，對於非線性系統來說，我們使用擴展卡爾曼濾波，區別在於EKF多了一個把預測和測量部分進行線性化的過程。

參考文章：

比如，在雷達中，人們感興趣的是跟蹤目標，但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響，得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波)，也可以是對於將來位置的估計(預測)，也可以是對過去位置的估計(插值或平滑)。

擴展卡爾曼濾波（EXTEND KALMAN FILTER, EKF）

擴展卡爾曼濾波器

是由kalman filter考慮時間非線性的動態系統，常應用於目標跟蹤系統。