卡爾曼濾波教學(xué) (2)

1、第六章卡爾曼濾波（The Kalman filtering） 1第一節(jié)卡爾曼濾波信號模型第二節(jié) 卡爾曼濾波方法第三節(jié) 卡爾曼濾波的應(yīng)用2.信號模型.狀態(tài)方程和量測方程維納濾波的模型：信號可以認(rèn)為是由白噪聲激勵(lì)一個(gè)線性系統(tǒng)的響應(yīng)，假設(shè)響應(yīng)和激勵(lì)的時(shí)域關(guān)系可以用下式表示： (6-52)上式也就是一階AR模型。 3在卡爾曼濾波中信號被稱為是狀態(tài)變量，用矢量的形式表示為，激勵(lì)信號也用矢量表示為，激勵(lì)和響應(yīng)之間的關(guān)系用傳遞矩陣來表示， 得出狀態(tài)方程： (6-53) 上式表示的含義就是在k時(shí)刻的狀態(tài)可以由它的前一個(gè)時(shí)刻的狀態(tài)來求得，即認(rèn)為k1時(shí)刻以前的各狀態(tài)都已記憶在狀態(tài)中了 4卡爾曼濾波是根據(jù)系統(tǒng)的量測

2、數(shù)據(jù)（即觀測數(shù)據(jù)）對系統(tǒng)的運(yùn)動進(jìn)行估計(jì)的，所以除了狀態(tài)方程之外，還需要量測方程。 在卡爾曼濾波中，用表示量測到的信號矢量序列，表示量測時(shí)引入的誤差矢量，則量測矢量與狀態(tài)矢量之間的關(guān)系可以寫成 (6-54)5上式和維納濾波的概念上是一致的，也就是說卡爾曼濾波的一維信號模型和維納濾波的信號模型是一致的。把式(6-55)推廣就得到更普遍的多維量測方程 (6-55)上式中的稱為量測矩陣，它的引入原因是，量測矢量的維數(shù)不一定與狀態(tài)矢量的維數(shù)相同，因?yàn)槲覀儾灰欢苡^測到所有需要的狀態(tài)參數(shù)。 6.信號模型根據(jù)狀態(tài)方程和量測方程，卡爾曼濾波的信號模型，如圖6.12所示。圖6.12 卡爾曼濾波的信號模型7【例6

3、-1】設(shè)卡爾曼濾波中量測方程為，已知信號的自相關(guān)函數(shù)的z變換為噪聲的自相關(guān)函數(shù)為，信號和噪聲統(tǒng)計(jì)獨(dú)立。求卡爾曼濾波信號模型中的和。8解：根據(jù)等式可以求得變換到時(shí)域得：因此又因?yàn)?，所?。9.卡爾曼濾波方法（The method of Kalman filtering）.卡爾曼濾波的一步遞推法模型把狀態(tài)方程和量測方程重新給出： (6-56) (6-57)假設(shè)信號的上一個(gè)估計(jì)值已知，現(xiàn)在的問題就是如何來求當(dāng)前時(shí)刻的估計(jì)值。 10用上兩式得到的和分別用和表示，得： (6-58) (6-59)必然，觀測值和估計(jì)值之間有誤差，它們之間的差稱為新息（innovation）： (6-60)顯然，新息的產(chǎn)生是

4、由于我們前面忽略了與所引起的 11用新息乘以一個(gè)修正矩陣，用它來代替式（656）的來對進(jìn)行估計(jì)： (6-61)由（656）（661）可以畫出卡爾曼濾波對進(jìn)行估計(jì)的遞推模型，如圖6.13所示 12輸入為觀測值，輸出為信號估計(jì)值。圖6.13 卡爾曼濾波的一步遞推法模型13.卡爾曼濾波的遞推公式從圖6.13容易看出，要估計(jì)出就必須要先找到最小均方誤差下的修正矩陣，結(jié)合式（661）、（656）、（657）得：(6-62)根據(jù)上式來求最小均方誤差下的，然后把求到的代入（661）則可以得到估計(jì)值。14設(shè)真值和估計(jì)值之間的誤差為：誤差是個(gè)矢量，因而均方誤差是一個(gè)矩陣，用表示。把式（662）代入得 (6-63

5、)均方誤差矩陣： (6-64)表示對向量取共軛轉(zhuǎn)置。 15為了計(jì)算方便，令 (6-65)找到和均方誤差矩陣的關(guān)系： (6-66)把式（663）代入式（664），最后化簡得： 16把式（666）代入（667）得令，代入上式化簡： (6-68)要使得均方誤差最小，則必須 17求得最小均方誤差下的修正矩陣為： (6-69)把上式代入(6-61)即可得均方誤差最小條件下的遞推公式。最小均方誤差為： (6-70)18綜上所述，得到卡爾曼濾波的一步遞推公式： (6-71) (6-72) (6-73) (6-74)19【例6-2】設(shè)卡爾曼濾波中量測方程為 已知信號的自相關(guān)函數(shù)的z變換為，噪聲的自相關(guān)函數(shù)為，

6、信號和噪聲統(tǒng)計(jì)獨(dú)立，已知在k0時(shí)刻開始觀測信號。試用卡爾曼濾波的公式求和，k0,1,2,3,4,5,6,7；以及穩(wěn)態(tài)時(shí)的和。解：由例6-6的結(jié)果知， 20把上式代入式(6-71) (6-74)得 （1） （2） （3） （4）求逆把（1）代入（2）、（3）式，消去，再把（2）和（3）聯(lián)立，得到 （5） 21初始條件為，k0開始觀測，利用等式（4），（5）進(jìn)行遞推得：k0，1.0000，1.0000，k1，0.5000，0.5000，k2，0.4048，0.4048，k3，0.3824，0.3824，k4，0.3768，0.3768，k5，0.3755，0.3755，k6，0.3751，0.37

7、51，k7，0.3750，0.3750，上面是遞推過程，還沒有達(dá)到穩(wěn)態(tài)的情況。 22假設(shè)到了某一時(shí)刻k1，前后時(shí)刻的均方誤差相等，也就是誤差不再隨著遞推增加而下降，達(dá)到最小的均方誤差了，即穩(wěn)態(tài)情況，式（5）中的誤差代入（5）式可以計(jì)算到穩(wěn)態(tài)時(shí)的均方誤差為：即穩(wěn)態(tài)時(shí)的修正矩陣，代入式4得穩(wěn)態(tài)時(shí)的信號估計(jì)：化到z域有：。23.卡爾曼濾波器的應(yīng)用(Application Kalman filter) 【例6-3】已知條件和例62一樣，狀態(tài)方程和測量方程為： 其中 ， 信號和噪聲統(tǒng)計(jì)獨(dú)立。求卡爾曼濾波器的穩(wěn)態(tài) 和 。24解：根據(jù)函數(shù)調(diào)用sysss（A，B，C，D，1），得到離散卡爾曼狀態(tài)模型，采樣周期

8、這里設(shè)為1。A，C已知，由于函數(shù)調(diào)用中是設(shè)計(jì)了兩個(gè)觀測信號的，我們這里只有一個(gè)觀測信號，所以B取0 1，后一個(gè)1表示噪聲 的系數(shù)。D取0。實(shí)際的語句如下：sys=ss(A,B,C,D,1)然后調(diào)用函數(shù)S，L，H，kalman（sys，Q，R），設(shè)計(jì)離散卡爾曼濾波器。實(shí)際語句和計(jì)算結(jié)果如下：s，l，h，=kalman(sys,0.36,1)l =0.3000 =0.6000h =0.3750 =0.3750這里省略了輸出的S，它表示的信息是達(dá)到穩(wěn)態(tài)后系統(tǒng)狀態(tài)模型，H和 表示系統(tǒng)穩(wěn)態(tài)的最終值 25有了修正矩陣和均方誤差，代入式（674）就可以根據(jù)觀測信號得到卡爾曼濾波的估計(jì)值了。從上面例題知道，只

9、要確定了狀態(tài)模型，就可以調(diào)用函數(shù)很快設(shè)計(jì)出卡爾曼濾波器，下面來看看卡爾曼濾波器在生物醫(yī)學(xué)信號中的應(yīng)用。在生物醫(yī)學(xué)信號處理中腦電圖的肌電偽跡和其它噪聲的消除，以及誘發(fā)電位的提取都有研究者嘗試用卡爾曼濾波器來處理。本節(jié)介紹卡爾曼濾波器在誘發(fā)電位提取中的應(yīng)用，方法如下：261.自發(fā)電位模型（EEG）和誘發(fā)電位（EP）模型的建立。如圖6.14所示，EEG信號通過用AR模型建立，激勵(lì)是白噪聲，EP信號的激勵(lì)是單位脈沖序列，用等式表示如下： 階AR模型d表示從該時(shí)刻開始有單位脈沖刺激。 27 圖6.14 EEG和EP模型28從圖6.14知道，觀測信號是EEG和EP的線性相加，用 表示第i次刺激后測量的信號，對M次測量平均得：疊加平均后的信號長度為N。利用先驗(yàn)知識建立好圖6.14的模型。假設(shè)單次誘發(fā)信號和平均誘發(fā)信號的關(guān)系是延時(shí)和幅度變化但波形一致的情況，即292.卡爾曼狀態(tài)方程和量測方程的建立。 其中X表示狀態(tài)變量，包括誘發(fā)信號、單位脈 沖信號、自發(fā)信號，長mpq1 A 是系統(tǒng)矩陣， 為輸入矩陣 是噪聲矩陣 是測量噪聲 是輸出矩陣 30有了上述方程后就可以利用卡爾曼濾波公式對 進(jìn)行估計(jì)，由于它包含多種狀態(tài)，誘發(fā)信號和它的關(guān)系為： 自發(fā)信號和估計(jì)值的關(guān)系為： 其中kmin（m，p