基于Matlab的Kalman濾波教學實踐

占榮輝，魯 敏

(國防科技大學電子科學與工程學院，湖南長沙 410073)

0 引言

雷達、聲納、導航、通信和自動控制等諸多應用中，需要對一個隨機動態(tài)系統(tǒng)的狀態(tài)進行估計，由一個測量裝置對系統(tǒng)狀態(tài)進行測量，通過記錄的測量值對狀態(tài)進行最優(yōu)估計。對于平隨機穩(wěn)過程，Wiener濾波器可得到線性系統(tǒng)狀態(tài)的最優(yōu)(最小均方誤差意義下)估計;而對于非線性系統(tǒng)或非平穩(wěn)環(huán)境，Wiener濾波卻不再適用。對于線性系統(tǒng)，只要噪聲是高斯型的，通過Kalman濾波總就能得到狀態(tài)的最優(yōu)估計，且這種估計過程是遞推進行的;即便是對于非線性系統(tǒng)，Kalman濾波器的擴展形式仍能適用于狀態(tài)估計(此時可能是次優(yōu)的)［1，2］。由于 Kalman 濾波在系統(tǒng)狀態(tài)估計、參數估計以及狀態(tài)—參數聯(lián)合估計等方面應用的廣泛性和表現出的突出優(yōu)勢，在高年級本科生或研究生開設的“統(tǒng)計信號處理”、“隨機信號處理”和“自動控制”或其它相關課程中都將其列為重點內容進行講解。

但是，Kalman濾波器的推導和應用涉及的數學理論強，概念抽象，且涉及到大量復雜的矩陣運算，在實際教學中學生往往感覺這一知識點晦澀難懂，對其中的分析方法和基本理論不能很好地理解與掌握。單純通過公式的機械記憶很難正確領會問題的本質，更談不上將Kalman濾波方法應用于解決實際問題。Matlab集數值運算、矩陣運算、符號運算和圖形處理等功能于一體。且具有涵蓋多個專業(yè)領域的工具箱函數，因此利用該軟件可以方便地建立系統(tǒng)數學模型，使得許多繁瑣的計算與圖形繪制等工作被簡單的軟件包或函數調用操作所代替［3－5］。

1 Kalman濾波方程推導

本文從最優(yōu)Bayes估計的理論公式出發(fā)導出Kalman濾波算式，可望從另一個角度對Kalman濾波方法作更直觀的解釋，同時拓寬學生的思維。

考慮下列方程所描述的線性系統(tǒng):

其中，xk為系統(tǒng)狀態(tài)(m維列向量);F為從k－1時刻到k的狀態(tài)轉移矩陣;H為觀測矩陣;wk和vk分別為m維和n維零均值過程噪聲和測量噪聲。

從Bayes估計的觀點看，濾波問題就是在給定觀測y1:k={yi}的條件下得到時變狀態(tài)量xk的估計，這要求建立后驗狀態(tài)分布 的概率密度函數PDF。假定初始PDF為p(x0|y0)=p(x0)，則理論p(xk|y1:k)可通過兩步遞推計算求得。

［預測］:假定k－1時刻狀態(tài)的后驗PDF設為p(x1:k－1)，則其一步前向預測的 PDF可以根據Chapman-Kolmogorov方程計算

［濾波:］在獲得 p(xk|y1:k－1)的基礎上，綜合新近的觀測yk，可通過如下的Bayes規(guī)則計算k時刻狀態(tài)的后驗PDF:

式(3)和式(4)構成了一個由k－1時刻后驗PDF向k時刻后驗PDF遞推的過程，但這只是理論上的遞推式。實際的求解過程依賴于具體的系統(tǒng)方程形式。若p(xk－1|y1:k－1)滿足高斯分布，且wk和vk為相互獨立的高斯噪聲(其協(xié)方差分別Qk和Rk)，則可得到xk的最大后驗估計為

對上式取對數，得到其等價的算式為

容易證明p(xk|y1:k－1)和p(xk|y1:k)可以通過解析遞推求得，且滿足高斯分布:

其中，

又因為

于是滿足式(6)的xk等價于

對上式進一步變形，可得

由于矩陣Pk/k－1和R均為對稱下定陣，故矩陣可逆，于是由式(14)可得

若定義

對上式進行適當的變形，可得

同理可得

將式(17)中的Kk代入式(18)，并采用矩陣求逆引理化簡，可得

于是有

由此可得完整的Kalman濾波遞推算式:

式中，Kk為卡爾曼增益，Sk為信息中vk=yk－的協(xié)方差距陣，上述結果與文獻［1］中采用正交化原理導出的算式完全一致。我們只要給一的定初始條件:，通過式(22)～式(25)進行遞推計算，即可動態(tài)得到狀態(tài)量xk的濾波估計。

2 Kalman濾波教學實例

2.1 標量情況—狀態(tài)預測

［例1］有一滿足AR(1)模型的隨機序列，其狀態(tài)演化過程可用差分方程表示為

用觀測方程yk=xk+vk對xk進行觀測，其中的wk和vk分別是方差為Q=1和R=9的零均值高斯白噪聲。試利用Kalman方程對xk進行預測，并給出狀態(tài)序列測量與預測的圖示結果。

假設初始狀態(tài)估計及估計方差分別為^x=23和P0=9，可以得到的Matlab預測結果如圖1和圖2所示。

由圖1和圖2的結果可以看出，經Kalman濾波得到的預測精度比單純的測量精度要高(說明了其在帶噪聲的測量中恢復信號的能力)。由式(22)～式(25)可知，濾波過程通過兩個步驟來完成:一是預測，二是修正，即先通過式(22)和式(23)獲得狀態(tài)及其方差的預測，再通過式(24)和式(25)完成狀態(tài)和方差的修正。且這兩個步驟是交替重復進行的。狀態(tài)的修正量△xk=Kkvk，故濾波結果既包含了(模型)預測的成份同時包含了測量成份，通過修正提高了估計精度，這一點在圖1的結果中得到了很好的體現。

下面通過對本例的理論分析與仿真結果進行比較，來說明Kalman濾波的收斂過程。由仿真條件及式(22)～式(27)的濾波公式可知:

圖1 Kalman濾波器預測過程

從P0/0=P0=9開始，通過式(28)～式(30)容易算出上面幾個參數的理論值，如表1所示。

表1 誤差方差與Kalman增益計算結果

若設Pk/k的穩(wěn)態(tài)值(即當k→時，誤差方差Pk/k趨近的穩(wěn)定值)為 珔P ，則應有 Pk/k=Pk－1/k－1=珔P，進一步根據式(30)可得

圖2 誤差方差和增益變化曲線

2.2 矢量情況—目標跟蹤

考慮某一維坐標軸中的跟蹤濾波問題，目標的運動狀態(tài)可描述為

式中，T為采樣間隔(觀測周期)，q2為擾動噪聲方差。

傳感器測量方程為

其中，H=［1 0］為測量矩陣，v∽N(0，R)為觀測噪聲。

［例2］目標初始位置x1=x0位于坐標原點，初始速度在［6，10］之間均勻分布，T=0.5，q=1 ，R=1。試給出目標位置的跟蹤過程，并評估跟蹤性能。

假設狀態(tài)估計及其誤差協(xié)方差矩陣采用“兩點法”初始化，即具有和:

得到的Matlab目標跟蹤仿真結果如圖3和圖4所示。

圖3 目標跟蹤濾波過程

圖4 目標跟蹤均方差

圖3和圖4給出了前20個時刻目標位置的演化過程及測量與跟蹤結果，這是通過5000次的Monte Carlo仿真得到的目標跟蹤的性能曲線。由圖中的結果可見，相比于直接測量(均方誤差約為1)，通過Kalman濾波處理后，其估計精度有了較大改善(圖中均方誤差約為0.75)，說明了Kalman濾波在目標跟蹤應用中的有效性和可行性。

3 結語

本文介紹了一種基于Matlab仿真的教學方法:我們避開了正交性原理等相關知識，從最優(yōu)Bayes濾波的角度出發(fā)，導出了Kalman濾波方程;在此基礎上結合兩個典型的應用實例，給出了完整的Kalman濾波算法實現程序及相應的實驗結果。

教學實踐證明，應用Matlab仿真工具輔助的Kalman濾波教學，可以有效幫助學生加深對基本概念和基本原理的理解，將其注意力集中于對基本分析方法的掌握上，從而真正領悟解決實際問題的思路和技巧。通過可視化工具的應用，使講授的內容形象化和具體化，有效提高了授課效率;通過具體實例的講解和現場仿真實現，擴展了學生的思維空間，提高了學生解決實際問題的能力，改善了教學質量。

［1］ Steven M.Kay著.羅-鵬飛等譯.統(tǒng)計信號處理基礎-估計與檢測理論［M］.北京:電子工業(yè)出版社，2003.

［2］ Eli Brookner.Tracking and Kalman Filtering Made Easy［M］.New York:John Wiley＆ Sons，1998.

［3］ 張剛，賀利芳，何方白等.基于Matlab的“數字信號處理”課程教學探索［J］.北京:高等教育研究，2007，24(2):45-46.

［4］ 杜世民，楊潤萍.Matlab在“信號與系統(tǒng)”教學中的應用研究［J］.南京:電氣電子教學學報，2009，31(6):89-91.

［5］ 羅軍輝，羅勇江，白義臣等.MATLAB7.0在數字信號處理中的應用［M］.北京:機械工業(yè)出版社，2005.

［6］ 張旭東，陳明泉.離散隨機信號處理［M］.北京:清華大學出版社，2005.