占榮輝，魯 敏

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

0 引言

雷達(dá)、聲納、導(dǎo)航、通信和自動控制等諸多應(yīng)用中，需要對一個隨機動態(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)估計、參數(shù)估計以及狀態(tài)—參數(shù)聯(lián)合估計等方面應(yīng)用的廣泛性和表現(xiàn)出的突出優(yōu)勢，在高年級本科生或研究生開設(shè)的“統(tǒng)計信號處理”、“隨機信號處理”和“自動控制”或其它相關(guān)課程中都將其列為重點內(nèi)容進行講解。

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

1 Kalman濾波方程推導(dǎo)

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

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

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

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

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

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

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

對上式取對數(shù)，得到其等價的算式為

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

其中，

又因為

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

對上式進一步變形，可得

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

若定義

對上式進行適當(dāng)?shù)淖冃危傻?/p>

同理可得

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

于是有

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

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

2 Kalman濾波教學(xué)實例

2.1 標(biāo)量情況—狀態(tài)預(yù)測

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

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

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

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

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

圖1 Kalman濾波器預(yù)測過程

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

表1 誤差方差與Kalman增益計算結(jié)果

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

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

2.2 矢量情況—目標(biāo)跟蹤

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

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

傳感器測量方程為

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

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

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

得到的Matlab目標(biāo)跟蹤仿真結(jié)果如圖3和圖4所示。

圖3 目標(biāo)跟蹤濾波過程

圖4 目標(biāo)跟蹤均方差

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

3 結(jié)語

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

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

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

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

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

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

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

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