卡爾曼濾波器在嵌入式控制系統中的應用

嵌入式控制系統要用卡爾曼濾波器來觀測進程中的變量，以便實現進程控制。本文將介紹卡爾曼濾波器設計和實現的基本原理，然後以汽車導航問題為實例，說明汽車位置控制過程中，如何利用卡爾曼濾波器這個有效工具對汽車的當前位置進行可靠的估計。

卡爾曼濾波器最初是專為飛行器導航而研發的，目前已成功應用在許多領域中。卡爾曼濾波器主要用來預估那些只能被系統本身間接或不精確觀測的系統狀態。

許多工程系統和嵌入式系統都需要濾波。例如收到受噪音干擾的無線通訊信號時，良好的濾波算法在保留有用資訊的同時，還可以從電磁信號中消除 噪音。又如在電源電壓中，未受干擾的電源是那些為了消除不期望波動而進行線電壓過濾的電源設備，如果不消除這些波動，將會縮短電子設備(如電腦和列印機) 的壽命。

卡爾曼濾波器適用於觀測進程中的變量，從數學角度看，卡爾曼濾波器可估計線性系統的狀態。卡爾曼濾波器不僅能在實踐中發揮很好的作用，而且 在理論上也頗引人注目，因為在各種濾波器中，卡爾曼濾波器的估計誤差最小。嵌入式控制系統經常需要使用卡爾曼濾波器，因為要控制一個進程，首先必須精確地 估計進程中的變量。

本文將介紹卡爾曼濾波器設計和實現的基本原理。文章首先引入卡爾曼濾波器算法並用該算法解決汽車導航問題。為了控制汽車的位置，首先必須對汽車的當前位置進行可靠的估計。卡爾曼濾波器正是具備這項功能的有效工具。

線性系統

線性系統

為了用卡爾曼濾波器消除信號中的噪音，被測量的進程必須能用線性系統描述。許多物理進程，如路上行駛的車輛、圍繞地球軌道運轉的衛星、由繞組電流驅動的電機軸或正弦射頻載波信號，均可用線性系統來近似。線性系統是指能用如下兩個方程描述的簡單進程：

狀態方程：

輸出方程：

在上述方程中，A、B和C均為矩陣，k是時間系數，x稱為系統狀態，u是系統的已知輸入，y是所測量的輸出。w和z表示噪音，其中變量w稱為進程噪音，z 稱為測量噪音，它們都是向量，因此包含多個元素。x中包含系統當前狀態的所有資訊，但它不能被直接測量。因此要測量向量y，y是受到噪音z干擾的系統狀態 x的函數。我們可以利用y獲取x的估計，但不一定必須藉由y的測量值來獲得x的估計，因為y受到了噪音的干擾。

例如在沿直線運動的汽車模型中，其狀態由汽車的位置p和速度v構成。輸入u是控制加速度，而輸出y則是測量位置。假定每隔T秒時間都能改變加速度並測量汽車的位置，根據物理學基本定律，速度v將由下述方程控制：

也就是說，下一時刻(T秒之後)的速度將等於當前速度加上控制加速度與T的乘積。但前面的方程並未給出v_k+1時刻的精確值，因為實際上該速度將受到陣風及其它意外噪音的干擾。這些速度噪音是隨時間變化的隨機變量，因此下述方程能更好地反映v的實際情況：

其中(方程17)表示速度噪音。類似地，可以推導出位置p的方程：

其中(方程18)表示位置噪音。下式提出了由位置和速度構成的狀態向量x：

最後，由於測量輸出是汽車的位置，可以得到線性系統的狀態方程如下：

z_k表示由儀器誤差等因素帶來的測量噪音。如果希望藉由反饋系統控制汽車，則需要得到位置p和速度v的精確估計，換句話說，即需要對狀態x進行估計。卡爾曼濾波器正是狀態估計的有效工具。

卡爾曼濾波器的原理和算法

假定線性系統的模型如前所述，我們希望利用測量得到的y來估計系統x的狀態。由於系統的行為遵循其狀態方程，而且可以獲得系統的位置測量， 那麼如何確定狀態x的最佳估計呢？我們需要能對實際狀態進行精確估計的預估器，儘管並不能直接測量該狀態。那麼預估器又應當滿足哪些準則呢？顯然必須滿足 以下兩個條件。

首先，狀態估計的平均值應等於實際狀態的平均值。換言之，我們並不希望估計出現這樣或那樣的偏差。在數學上，人們總希望估計的期望值應等於狀態的期望值。

其次，我們希望狀態估計與實際狀態之間的偏差儘可能小。也就是說，不僅希望狀態估計的平均值等於實際狀態的平均值，而且希望預估器的狀態估計與實際狀態之間的偏差盡可能達到最小。在數學上，人們總希望預估器的誤差方差盡可能最小。

卡爾曼濾波器正是能滿足以上兩條準則的預估器，但卡爾曼濾波器解決方案也必須滿足特定的噪音條件。在系統模型中，w表示進程噪音，而z表示測量噪音。因此，必須假定w和z的平均值均為0且w與z不相關。這樣，在任何時刻k、w_k和x_k均為不相關的隨機變量，而噪音協方差矩陣S_w和S_z分別為：

進程噪音協方差：

測量噪音協方差：

其中w_T和z_T分別表示隨機噪音向量w和z的轉置，E(‧)表示期望值。

現在就可以開始研究卡爾曼濾波器方程了。首先，可以選擇不同的等價方程來描述卡爾曼濾波器，其中一種表述如下：

卡爾曼濾波器方程由3個方程組成，每個方程都包含矩陣運算。在上述方程中，a^-1表示矩陣求逆，而a^T上表示矩陣轉置。K矩陣稱為卡爾曼增益，而P矩陣表示估計誤差的協方差。

上述狀態估計方程相當直觀。方程中用來推導k + 1時刻狀態估計的第一項正好等於A與k時刻狀態估計的乘積加上B與k時刻已知輸入的乘積。如果不再進行測量，那麼該值就是系統的狀態估計。換言之，狀態估 計可以像系統模型中的狀態向量一樣傳遞至後續時刻。方程中第二項稱為校正項，表示在測量條件下，用於對傳遞的狀態估計進行修正的校正量。

K方程的校驗表明：測量噪音較大，S_z也將較大，因此K值應當較小，而且在計算下一時刻的測量值y時，不應過分信賴該值；另一方面，測量噪音較小，S_z也將較小，這樣K值應當較大，而且在計算下一時刻的測量值時，可以充分信賴該值。

汽車導航應用實例

現在考慮先前提出的汽車導航問題。汽車沿直線行駛，位置測量的誤差為10英尺(一個標準偏差)；命令加速度是常數，其值為1英尺/秒²；加速度噪音為0.2英尺/秒² (一個標準偏差)；位置測量的周期為0.1秒(T=0.1)。那麼如何得到運動汽車位置的最佳估計？由於存在較大的測量噪音，可以肯定計算結果比測量值更好。

因為T=0.1，表徵系統的線性模型可用本文提出的系統建模方法推導得到：

因為測量噪音的標準偏差為10英尺，S^z矩陣可簡單地取值為100。

現在推導S^w矩陣。因為位置與加速度的0.005倍成正比並且加速度噪音為0.2英尺/秒²，因此位置噪音的偏差為(0.005)²×(0.2) ²= 10^-6。類似地，因為速度與加速度的0.1倍成正比，因此速度噪音的偏差為(0.1)²×(0.2)²=4×10^-4。最後，位置噪音和速度噪音的協方差等於位置噪音的標準方差與速度噪音的標準方差的乘積，計算結果為(0.005×0.2)×(0.1×0.2)=2×10^-5。將以上計算結果相結合，即可得到矩陣S^w：

接著，為位置和速度設置最佳的初始值，並將(方程16)初始化為最初估計的誤差。然後每隔一個時間步長執行一次卡爾曼濾波器算法。

我們使用Matlab模擬卡爾曼濾波器，模擬結果參見以下各圖。圖1顯示了車輛的實際位置和估計位置，兩條平滑曲線表示實際位置和估計位置，由於相隔太近而難以區分。較為粗糙的曲線表示測量位置。

圖2顯示了實際位置和測量位置之間的誤差以及實際位置與卡爾曼濾波器估計位置之間的誤差。測量誤差的標準偏差約為10英尺，偶爾也會出現30英尺(3Σ)的峰值。估計位置的誤差則一直保持約為2英尺。

圖3顯示了卡爾曼濾波器的優勢。因為汽車的速度是狀態x的一部份，因此我們得到位置估計的同時也得到了速度估計。圖4顯示了實際速度和卡爾曼濾波器估計速度之間的誤差。

得到上述結果的Matlab程式如表1所示。Matlab是一種很容易理解的語言，很像偽碼(pseudocode)，但具有內置矩陣運算功能。使用Matlab執行程式時，每次得到的結果會有所不同，這是因為模擬中存在隨機噪音，但總體上與這裏給出的各圖都很相近。

實際問題及其擴展

卡爾曼濾波器的基本原理很簡單，但濾波器方程對矩陣代數的依賴性很強。表2顯示了用C語言描述的卡爾曼濾波器修正方程。表2中的矩陣代數參 考可見www.embedded.com/code.html。該網站列出的程式非常全面，如果問題足夠小，程式還能相應簡化。例如，矩陣的轉置：

等於：

因此，如果需要對矩陣進行轉置，就可使用上述方程。矩陣運算的補充C代碼和卡爾曼濾波可參見http: //wad.www.media.mit.edu/people/wad/mas864/proj_src.html。具有超過3個狀態的系統將顯著增加 程式的代碼長度和計算量。與矩陣轉置相關的計算量正比於n³(這裏n表示矩陣的維數)。這意味著，如果卡爾曼濾波器的狀態數目增加 1倍，計算量將增至原來的8倍。對於一個維數適中的卡爾曼濾波器，幾乎所有的處理工作都花在矩陣運算上。但穩態卡爾曼濾波器在提供良好預估性能的同時，還 能極大地削減運算量，因此也不必過於擔心。在穩態卡爾曼濾波器中，矩陣K_k和P_k均為常量，因此在代碼中可定義為常值，而卡爾曼濾波器方程中唯一需要實時實現的是(方程15)方程，由簡單的乘積和加法運算組成(如果使用的是DSP，則由乘積和累加運算組成)。

我們已經討論了線性系統的狀態估計，但如何估計非線性系統的狀態呢？事實上，在工程中幾乎所有的進程都是非線性的。一些非線性系統非常接近 線性系統，而另一些則差別甚大。這一點早在卡爾曼濾波器發展初期階段就引起了重視，因而推出了“擴展卡爾曼濾波器”。擴展卡爾曼濾波器只是簡單地將線性卡 爾曼濾波原理推廣到非線性系統。

到目前為止，我們已經逐步討論了藉由測量對狀態進行估計的步驟。獲得整個測量時序記錄後，如果希望將狀態作為時間的函數進行預估，又應當如 何處理呢？例如在上述示例中，如何在獲得必要資訊的基礎上，重構汽車的行駛軌跡呢？我們似乎完全可以比採用卡爾曼濾波器做得更好，因為在k時刻預估系統狀 態時，我們不僅能測量k時刻之前(包括k時刻)的狀態，還能測量k時刻之後的狀態。為此必須修正卡爾曼濾波器，這就是卡爾曼平滑濾波器的由來。

卡爾曼濾波器不僅很實用，其工作原理也很有吸引力。卡爾曼濾波器可減小估計誤差的偏差，但如果我們更關心最壞情況下的估計誤差，或者希望減 小“最壞”的估計誤差，而不是“平均”誤差時，問題又該如何解決呢？H∞濾波器可以解決這個問題。H∞濾波器(讀作“H無限大”濾波器，有時寫作H∞ )是上世紀八十年代開發出的卡爾曼濾波替代方案。H∞濾波器應用不如卡爾曼濾波器廣泛，因此也不像卡爾曼濾波器那樣廣為人知，但在某些情況下更為有效。

卡爾曼濾波理論假定進程噪音w與測量噪音z互不相關。如果在系統中進程噪音和測量噪音相關，那麼應當如何處理呢？這就是相關噪音問題，同樣可以藉由修正卡爾曼濾波器來解決這個問題。此外，卡爾曼濾波器還需要明確的噪音協方差S_w和S_z。如果S_w和S_z未知，又該如何處理呢？我們如何能夠獲得狀態的最佳估計？這需要再次求助於H∞濾波器。

卡爾曼濾波涉及的領域很廣泛，不是本文就能盡述的。自1960年問世以來，業界已經發表了數千篇有關卡爾曼濾波的論文，出版了數十本教材。

發展歷史及其展望

卡爾曼濾波器最初由Rudolph Kalman開發設計，其研究成果發表在一家著名的期刊上，因為它比其它人的研究成果更為通用也更完整，因而被命名為卡爾曼濾波器，有時也被稱為 Kalman-Bucy濾波器，因為Richard Bucy早期曾與Kalman共同研究該濾波器。

卡爾曼濾波算法的起源可以追溯到1795年由年僅18歲的Karl Gauss提出的最小二乘理論。卡爾曼濾波算法與許多新技術一樣，也是致力於解決特定問題，例如阿波羅空間計劃中的太空梭導航問題。從此，卡爾曼濾波器逐 漸應用到許多領域，包括各種導航(航空太空、陸地和海洋)、核電站設備、人口統計建模、製造業、地層放射性探測以及模糊邏輯和神經網路學習。

Dan Simon是Cleveland大學電子工程和電腦工程系的教授和工業顧問。他的教學和研究方向包括濾波、控制理論、嵌入式系統、模糊邏輯和神經網路。目 前，他正試圖利用卡爾曼濾波器實現基於DSP的電機控制器，可以藉由電子郵件：d.j.simon@csuohio.edu與他聯繫。

參考文獻

1. Gelb, A. Applied Optimal Estimation. Cambridge, MA: MIT Press, 1974。這是一本“過時但仍然叫座”的參考書，它由麻省理工大學出版社出版，內容淺顯易懂，不僅從介紹最基本的原理入手，而且還非常注重實際應用。

2. Anderson, B. and J. Moore. Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall, 1979。這是一本純數學參考書，非常深奧，但有助於理解卡爾曼濾波算法及相關問題的基本原理。

3. Grewal, M. and A. Andrews. Kalman Filtering Theory and Practice. Englewood Cliffs, NJ: Prentice-Hall, 1993。本書介於以上兩本參考書之間，在理論和實踐之間架起了一座橋樑。本書的一大特色是磁片中包含卡爾曼濾波算法的原始碼，不足之處在於原始碼是用 Fortran語言編寫的。

4. Sorenson, H. Kalman Filtering: Theory and Application. Los Alamitos, CA: IEEE Press, 1985。這是一本有關卡爾曼濾波的經典論文集，包括Kalman 1960年發表的原始論文。雖然本文集偏向學術研究，但也有助於讀者了解卡爾曼濾波原理的發展歷程。

5. http://ourworld.compuserve.com/homepages/PDJoseph/是 Peter Joseph的網址，提供了許多有關卡爾曼濾波的專題資源。Joseph博士自1960年開始，長期從事卡爾曼濾波器研究，並於1968年合著了最早的有 關卡爾曼濾波的教材。Peter Joseph的網站還包括卡爾曼濾波的初級、中級和高級教程。