維納濾波器與卡爾曼濾波器

1、 ( )( )( )x ns nw n (7-1)則輸出( )y n為( )( )( )( ) ()my nx nh nh m x nm (7-2)()()(nwnsnx)( )(nsny )(nh圖7-1 維納濾波器的輸入輸出關(guān)系)( )()(nsnsne (7-3)22)( )()(nsnsEneE(7-4) 0n當(dāng)0,h(n)0, 0)(nnh當(dāng)0mm)h(m)x(n(n)s y(n)(7-5)202) )()()()(mmnxmhnsEneE(7-6)2 , 1 , 00)() )()()(20jjnxmnxmhnsEmopt(7-7)即0)()()()()(0jjnxmnxEmhjn

2、xnsEmopt(7-8)用相關(guān)函數(shù)R來表達上式，則得到維納霍夫方程的離散形式：0)()()(0jmjRmhjRmxxoptxs從維納霍夫方程中解出的h就是最小均方誤差下的最佳h， 即)(nhopt 求到 )(nhopt，這時的均方誤差為最?。?20min2) )()()()(moptmnxmhnsEneE )()()()()()()(2)(0002mroptoptmrnxrhmnxmhmnxmhnsnsE000)()()()()(2)0(mrxxoptoptmxsoptssrmRrhmhmRmhR由式（7-9）進一步化簡得：0min2)()()0()(mxsoptssmRmhRneE(7-1

3、0)10)()()( )(Nmmnxmhnsny (7-11)2102) )()()()(NmmnxmhnsEneE (7-12) 12 , 1 , 00)() )()()(210NjjnxmnxmhnsENmopt(7-13)1, 1 , 0)()()()()(10NjjnxmnxEmhjnxnsENmopt .(7-14)1, 2 , 1 , 0)()()(10NjmjRmhjRNmxxoptxs (7-15)于是得到N個線性方程：)0() 1()2() 1 () 1()0() 1(1)2() 1()0() 1 () 1 ()0() 1 (1) 1() 1() 1 () 1 ()0()0(

4、)0(0 xxxxxxxsxxxxxxxsxxxxxxxsRNhNRhNRhNRNjNRNhRhRhRjNRNhRhRhRj寫成矩陣形式有： ) 1() 1 ()0() 1() 1 ()0()0()2() 1()2()0() 1 () 1() 1 ()0(NRRRNhhhRNRNRNRRRNRRRxsxsxsxxxxxxxxxxxxxxxxxx(7-16)簡化形式：RxxH=Rxs (7-17)式中，Hh(0) h(1) h(N-1)，是待求的單位脈沖響應(yīng)；Rxs) 1(),1 (),0(NRRRxsxsxs，是互相關(guān)序列； Rxx )0()2() 1()2()0() 1 () 1() 1 (

5、)0(xxxxxxxxxxxxxxxxxxRNRNRNRRRNRRR，是自相關(guān)矩陣 只要Rxx是非奇異的，就可以求到H：H=Rxx1 Rxs (7-18)求得)(nhopt后，這時的均方誤差為最?。?210min2) )()()()(NmoptmnxmhnsEneE )()()()()()()(2)(1010102NmNroptoptNmrnxrhmnxmhmnxmhnsnsE101010)()()()()(2)0(NmNrxxoptoptNmxsoptssrmRrhmhmRmhR 10min2)()()0()(NmxsoptssmRmhRneE (7-19)用有限長的)(nh來實現(xiàn)維納濾波時

6、，當(dāng)已知觀測值的自相關(guān)和觀測值與信號的互相關(guān)時就可以按照式（7-15）在時域里求解 )(nhopt但是當(dāng)N比較大時，計算量很大，并 且涉及到求自相關(guān)矩陣的逆矩陣問題。 )(ns)(nw0)()(mRmRwssw則有)()()()()()()()(mRmnsnwmnsnsEmnsnxEmRssxs)()()()()()()(mRmRmnwmnsnwnsEmRwwssxx則式（7-15）和式（7-19）化為：1, 2 , 1 , 0)()()()(10NjmjRmjRmhjRwwNmssoptss(7-20) 10min2)()()0()(NmssoptssmRmhRneE (7-21)【例7-1

7、】已知圖7-1中 )()()(nwnsnx且 )(ns與 )(nw統(tǒng)計獨立，其中 )(ns的自相關(guān)序列為 mssmR6 . 0)()(nw， 是方差為1的單位白噪聲，維納濾波器來估計 )(ns，并求最小均方誤差。 解依題意，已知信號的自相關(guān)和噪聲的自相關(guān)為： mssmR6 . 0)()()(mmRww，代入式（7-20）得 ) 1 (2)0(6 . 06 . 01) 1 (6 . 0)0(210hhjhhj解得： )0(h0.451， ) 1 (h0.165。 將上述結(jié)果代入式（7-21），求得最小均方誤差：45. 0) 1 (6 . 0)0(1)()()0()(10min2hhmRmhRne

8、Emssss若要進一步減小誤差可以適當(dāng)增加維納濾波的階數(shù)，但相應(yīng)的計算量也會增加。 提出的白化的方法求解維納霍夫方程，得到系統(tǒng)函數(shù) H(z)。 由第三章的知識，我們知道隨機信號都可以看成是由一白色噪聲 w1(n) 激勵一個物 理可實現(xiàn) 的系統(tǒng) 或模型的響應(yīng)，如圖7-2所示， 其中A(z) 表示系統(tǒng) 的傳遞函數(shù)。由于 x(n) = s (n) + w(n)， 在圖7-2的基礎(chǔ)上給出x(n)的信號模型 )(ns)(zA)(1nw 圖7-3 )(nx的信號模型 )(ns)(ns)(zA)(1nw)(nw)(nx圖7-4 維納濾波器的輸入信號模型)(nx)(zB)(1nw)()(2111mmRwww2

9、1w)(mRss )()()()()()()(11rkssrmnwraknwkaEmnsnsEmR)()()(11rkmRrakawwkr令 krl上式 lkwwwwkllkakalmRlmRlkaka)()()()()()(1111令 )()()()()(lalalkakalfl)()()()()()()()(111111mamamRmfmRlflmRmRwwwwlwwss對式（7-22）進行Z變換得到系統(tǒng)函數(shù)和相關(guān)函數(shù)的z變換之間的關(guān)系：)()()(121zAzAzRwss (7-23)同樣，對圖7-4進行z變換得)()()(121zBzBzRwxx(7-24)圖7-4中利用卷積性質(zhì)還可以

10、找到互相關(guān)函數(shù)之間的關(guān)系：)()()()()()(1mnsknwkbEmnsnxEmRkxs)()()()(11mbmRkmRkbswksw兩邊z變換得到)()()(11zBzRzRswxs (7-25)如果已知觀測信號的自相關(guān)函數(shù)，求它的z變換， 然后找到該函數(shù)的成對零點、極點，取其中在單位圓內(nèi)的那一半零點 、極點構(gòu)成)(zB，另外在)(zB)(1zB單位圓外的零、極點構(gòu)成，這樣就保證了 是因果的，并且是最小相位系統(tǒng)。從圖7-4可得)()(1)(1zXzBzW (7-26)由于系統(tǒng)函數(shù))(zB的零點和極點都在單位圓內(nèi)，)(1zB)(nx即是一個物理可實現(xiàn)的最小相位系統(tǒng)，則也是一個物理可實現(xiàn)的

11、最小相移網(wǎng)絡(luò)函數(shù)。我們就可以利用式（7-26）對 )(nx)(1nw)(1zB進行白化，即把當(dāng)作輸入，當(dāng)作輸出，是系統(tǒng)傳遞函數(shù)。)(zH)(zHopt)(1zB)()()(zBzGzH (7-27)()()(nwnsnx)( )(nsny)(nh（a）)(nx)( )(nsny)(1zB)(zG)(1nw（b）圖7-5 利用白化方法求解模型)(nx)(mRxx)(zRxx)()()(121zBzBzRwxx)(zB)()()(zBzGzH)(zB)(zHopt01)()()( )(mmnwmgnsny (7-28)均方誤差為：2012) )()()()(mmnwmgnsEneE )()()()

12、()()()(2)(0011012mrmrnwrgmnwmgmnwmgnsnsE000)()()()()(2)0(111mrwwmswssrmRrgmgmRmgR)()(2111mmRwww02202)()()(2)0()(11mwmswssmgmRmgRneE02202)(1)()()0(11111mswwmwswwssmRmRmgR均方誤差最小也就是上式的中間一項最小，所以0,)()(211mmRmgwswopt (7-30)注意，這里的)(mg是因果的。對該式求z變換，得到211)()(wswoptzRzG (7-31)(1zRsw)(1mRsw表示對求單邊z變換。所以維納霍夫方程的系統(tǒng)

13、函數(shù)解表示為：)()()(zBzGzHoptopt)()(211zBzRwsw由式（7-25）上式可以表示為：)()()(zBzGzHoptopt)()(211zBzRwsw)()(/ )(211zBzBzRwxs因果的維納濾波器的最小均方誤差為： min2)(neE022)(1)0(11mswwssmRRmswwssmumRR)()(1)0(2211 (7-33)利用帕塞伐爾(Parseval)定理，上式可用z域來表示：min2)(neEcxsoptsszdzzRzHzRj)()()(211(7-34) min2)(neEcxsoptsszdzzRzHzRj)()()(211 (7-34)(

14、)()(nwnsnx)(ns)(nw)(nsmssmR8 . 0)()(nw)(ns【例7-2】已知圖7-1中，且統(tǒng)計獨立，其中的自相關(guān)序列為，是方差為1的單位白噪聲，試，并求最小均方誤差。與設(shè)計一個物理可實現(xiàn)的維納濾波器來估計mssmR8 . 0)()()(mmRww0)(mRsw)()(mRmRssxs解依題意，已知，步驟1)()()(mRmRmRwwssxx求z變換25. 18 . 0 ,)8 . 01)(8 . 01 ()5 . 01)(5 . 01 (6 . 11)8 . 01)(8 . 01 (36. 0)(111zzzzzzzzRxx步驟2由于)()()(121zBzBzRwxx

15、，容易找到最小相位系統(tǒng)和白噪聲方差zzzzB8 . 0 ,8 . 015 . 01)(1125. 1,8 . 015 . 01)(1zzzzB6 . 121w步驟3利用式（732） )(zHopt)()(/ )(211zBzBzRwxs)5 . 01)(8 . 01 (36. 0)5 . 01 (6 . 18 . 01111zzzz對括號里面求反變換，注意括號內(nèi)的收斂域為28 . 0 z)5 . 01)(8 . 01 (36. 011zzZ) 1()2(6 . 0)()8 . 0(6 . 0nununn取因果部分，也就是第一項，所以118 . 0116 . 0)5 . 01)(8 . 01 (

16、36. 0zzz)(zHopt0,)5 . 0(375. 0)(nnhn步驟4最小均方誤差為 min2)(neEcxsoptsszdzzRzHzRj)()()(211cdzzzzzj)5 . 0)(25. 1)(8 . 0()5 . 0625. 0(45. 021取單位圓為積分圍線，有兩個單位圓內(nèi)的極點，0.8和0.5，求它們的留數(shù)和，所以min2)(neE375. 0)25. 15 . 0)(8 . 05 . 0()5 . 05 . 0*625. 0(45. 0)5 . 08 . 0)(25. 18 . 0()5 . 08 . 0*625. 0(45. 0)(nx) 1( nx)2( nx)

17、( )(nsny)( )(Nnsny0)(nyd)(ny)()()(nwnsnx)( )(Nnsny)(nh)()(Nnsnyd圖7-6維納預(yù)測器本節(jié)和上節(jié)一樣著重討論預(yù)測器的系統(tǒng)函數(shù)以及預(yù)測的均方誤差，維納預(yù)測器和維納濾波器比較類似，因而分析方法也都可以借鑒前面的內(nèi)容。 )(nh0, 0)(nnh當(dāng)對于圖7-6模型，設(shè)是物理可實現(xiàn)的，也即，則有是因果序列：0)()()( )(mmnxmhNnsny (7-35)202) )()()()(mmnxmhNnsEneE (7-36)要使得均方誤差最小，則將上式對各)(mh，m0，1，求偏導(dǎo)，并且等于零，得：)(mh2 , 1 , 00)() )()

18、()(20jjnxmnxmhNnsEmopt (7-37) 用相關(guān)函數(shù)R來表達上式：0)()()()()(0jjnxmnxEmhjnxNnsEmopt(7-38)0)()()(0jmjRmhjNRmxxoptxs (7-39)()(Nnsnyd)()()()(NmRNmnsnxEmRxsxyd由于，則，z變換得)()(zRzzRxsNxyd)()(11zRzzRxsNxyd (7-40)()(Nnsnyd)()(nsnyd借鑒維納濾波器的結(jié)果類似給出維納預(yù)測器的最佳傳遞函數(shù)，對應(yīng)維納預(yù)測器，對應(yīng)維納濾波器，故因果的預(yù)測器的傳遞函數(shù)為： )(zHopt)()(/ )(211zBzBzRwxyd)

19、()(/ )(211zBzBzRzwxsN (7-41)最小均方誤差為 min2)(NneE022)(1)0(11mywwssmRRd (7-42)利用帕塞伐爾(Parseval)定理，上式可用z域來表示min2)(NneEcxyoptsszdzzRzHzRjd)()()(211cxsNoptsszdzzRzzHzRj)()()(211)()()(nwnsnx)(ns)(nw)(nsmssmR8 . 0)()(nw) 1( ns【例7-3】已知圖7-6中，且與統(tǒng)計獨立，其中的自相關(guān)序列為，是方差為1的單位白噪聲，并求最小均方誤差。試設(shè)計一個物理可實現(xiàn)的維納預(yù)測器估計mssmR8 . 0)()(

20、)(mmRww0)(mRsw解依題意已知， )()(mRmRssxs)()()(mRmRmRwwssxx求z變換：25. 18 . 0 ,)8 . 01)(8 . 01 ()5 . 01)(5 . 01 (6 . 11)8 . 01)(8 . 01 (36. 0)(111zzzzzzzzRxx由于)()()(121zBzBzRwxx，容易找到最小相位系統(tǒng)和白噪聲方差：zzzzB8 . 0 ,8 . 015 . 01)(1125. 1,8 . 015 . 01)(1zzzzB6 . 121w由式（7-41），N1，)(zHopt)()(/ )(211zBzBzzRwxs)()(/ )(211zB

21、zBzzRwss求z變換：25. 18 . 0 ,)8 . 01)(8 . 01 ()5 . 01)(5 . 01 (6 . 11)8 . 01)(8 . 01 (36. 0)(111zzzzzzzzRxx由于)()()(121zBzBzRwxx，容易找到最小相位系統(tǒng)和白噪聲方差：zzzzB8 . 0 ,8 . 015 . 01)(1125. 1,8 . 015 . 01)(1zzzzB6 . 121w由式（7-41），N1，)(zHopt)()(/ )(211zBzBzzRwxs)()(/ )(211zBzBzzRwss)5 .01)(8 .01 (36.0)5 .01 (6 .18 .01

22、111zzzzz對括號里面求z反變換，注意括號內(nèi)的收斂域為：28 . 0 z，)5 . 01)(8 . 01 (36. 011zzzZ) 1()2(2 . 1)()8 . 0(48. 0nununn取因果部分，也就是第一項，所以118.01148.0)5.01)(8.01(36.0zzzz)(zHopt11115 . 013 . 0)8 . 01 (48. 0)5 . 01 (6 . 18 . 01zzzz0,)5 . 0(3 . 0)(nnhn把上式寫成差分方程形式有：)( 5 . 0)(3 . 0) 1( nsnxnsmin2)(neEcssoptsszdzzRzzHzRj)()()(21

23、11最小均方誤差為：cdzzzj6 . 0)8 . 01)(5 . 0(36. 021)(nw)(Nns)()(nsnx)( )(Nnxny)(nh)()(Nnxnyd圖7-7 N步純預(yù)測器)()(nsnx)()()(121zBzBzRwxx)()()()()(121zBzBzRzRzRwxsssxx這時，用白化法來求解預(yù)測器的系統(tǒng)函數(shù)。因為，從而有： (7-44)(zHopt)()(/ )(211zBzBzRzwxsN)()()()(/ )()(211211zBzBzzBzBzBzBzNwwN將上式代入式（7-41）、（7-43）得：min2)(NneEcxsNoptsszdzzRzzHzR

24、j)()()(211cNNwzdzzBzzBzzBzBj)()()()(21121假設(shè)B(z）是b（n）的z變換，且b（n）是實序列，則上式可以利用帕塞伐爾定(Parseval)理進一步化簡：min2)(NneE )()()()(221nnwNnbnuNnbnb (7-46)又因為B(z）是最小相位系統(tǒng)，一定是因果的，上式可以簡化min2)(NneE102202022)( )()(11NnwnnwnbNnbnb (7-47)上式說明最小均方誤差隨著N的增加而增加，也即預(yù)測距離越遠誤差越大。 )()(nsnx)(nsmssmR8 . 0)()( NnsmssmR8 . 0)(【例7-4】已知圖7

25、-7中，其中的自相關(guān)序列為，試設(shè)計一個物理可，并求最小均方誤差。，則實現(xiàn)的維納預(yù)測器來估計解依題意，已知25. 18 . 0 ,)8 . 01)(8 . 01 (36. 0)(1zzzzRss因為 )()()()()(121zBzBzRzRzRwxsssxx容易找到最小相位系統(tǒng)和白噪聲方差:zzzB8 . 0 ,8 . 011)(1)(8 . 0)(nunbn25. 1,8 . 011)(1zzzB36. 021w利用式（7-45）:)(zHopt8 . 011)8 . 01 ()()(11zzzzBzBzNN)(8 . 08 . 01111NnuzzZNnN0n因為 ，只取的部分，有:)(8

26、 . 08 . 01111nuzzZNnN)8 . 0118 . 08 . 01111zzzNN)(zHoptNoptzH8 . 0)(回到z域有：，代入得:最小均方誤差為：min2)(NneENNnnNnwnunb210210228 . 01)(8 . 036. 0)(1它說明當(dāng)N越大，誤差越大，當(dāng)N0時，沒有誤差。把上述結(jié)果用模型表示如圖7-8所示。)()(nsnx)(8 . 0)( )(nsNnsnyN)(8 . 0)(nnhNopt圖7-8 例題7-3的純預(yù)測模型0)()()( )(mmnxmhNnxnypmmnxmhnxny1)()()( )(這就是一步線性預(yù)測公式，常常用下列符合表

27、示pmpmmnxanx1)()( (7-48)式中p為階數(shù)，)(mhapm。預(yù)測的均方誤差為:) )()()(212pmpmmnxanxEneEpmplxxplpmpmxxpmxxmlRaamRaR111)( )( 2)0( (7-49)要使得均方誤差最小，將上式右邊對pma求偏導(dǎo)并且等于零，得到p個等式 pmaplmlRalRpmxxpmxx, 2 , 10)()(1 (7-50)最小均方誤差：pmxxpmxxmRaRneE1min2)()0()( (7-51)式（7-50）就是YuleWalker（Y-W）方程，和第三章AR模型參數(shù)估計的方程一致，如何去求解該方程在第三章有詳細介紹。把Yu

28、leWalker（Y-W）方程和維納霍夫方程進行比較，維納霍夫方程要估計的量是s(n)，Y-W方程要估計的量是x(n)本身，因而解維納霍夫方程要已知x(n)、y(n)的互相關(guān)函數(shù)，實際中這個互相關(guān)函數(shù)往往是未知的，而解Y-W方程只需要知道觀測信號的自相關(guān)函數(shù)。因此Y-W方程比W-H方程更具有實用價值。)( nsmssmR8 . 0)(mssxxmRmR8 . 0)()(64. 0)2(, 8 . 0) 1(, 1)0(xxxxxxRRR例7-5】已知圖7-7中x(n)=s(n)，其中的自相關(guān)序列為的可實現(xiàn)的一步線性預(yù)測器，并求最小均方誤差。解， ，試設(shè)計一個p2利用Y-W方程2 , 10)()

29、(21lmlRalRmxxpmxx，可以列出2個方程式08 . 064. 008 . 08 . 02121ppppaaaa0, 8 . 021ppaa) 1(8 . 0)( nxnx解得：，也即36. 064. 01)()0()(1min2pmxxpmxxmRaRneE結(jié)果和例（7-4）N=1時一致。),(),(),(ftWftSftXii),(),(),(1),(1ftWftSftXNftXNiiiNitwNtstxiNi, 2 , 1),(1)()(1對每次觀測用短時傅立葉變換求時頻表示（TFR）：對N次觀測的時頻表示（TFR）求平均：，樣本平均的時頻表示（TFR）為： (1)樣本平均為：

30、),(1),(),(ftWNftSftX (2)從式（2）可以得到一個基于樣本平均的簡單時頻平面后驗維納濾波器：),(1),(),(),(ftWNftSftSfth (3) ),(1),(),(1),(),(),(11NiiiNiiiftXIFNftWftSCOVNftWftSftX .(4),(),(),(),(),(),(ftXIFftWftSCOVftWftSftX (5),(ftW),(ftWi式中COV表示信號和噪聲之間的方差，也就是考慮了信號和噪聲并非相互獨立；IF是干擾項；表示樣本平均的噪聲功率；表示樣本噪聲功率的平均。)(nx),2(),1(nxnx)(ns)(zH)(nh)

31、1( ns)(nx)( ns它是根據(jù)前一個估計值和最近一個觀測值來估計信號的當(dāng)前推方法進行估計的，因而卡爾曼濾波對信號的平穩(wěn)性和時不變性不做要求。我們利用維納濾波的模型引入到卡爾曼濾波的信號模型。，它是用狀態(tài)方程和遞) 1() 1()(1nwnasns)(ns)(1nw)(zA上式也就是一階AR模型。在卡爾曼濾波中信號)(ns被稱為是狀態(tài)變量，用矢量的 S(k)S(k)形式表示為，在k時刻的狀態(tài)用表示， 在k1時刻的狀態(tài)用1)S(k 表示。 )(1nw(k)w1A(k)激勵信號也用矢量表示為，激勵和響應(yīng)之間的關(guān)系用傳遞矩陣來表 示，它是由系統(tǒng)的)(zA有一定關(guān)系。有了這些假設(shè)后結(jié)構(gòu)確定的，與我

32、們給出狀態(tài)方程：1)(kw1)A(k)S(kS(k)1 (7-53)S(k)1)S(k 1)S(k 上式表示的含義就是在k時刻的狀態(tài)可以由它的前一個時刻的狀態(tài)來求得，即認為k1時刻以前的各狀態(tài)都已記憶在狀態(tài)中了 )(ns)(zA)(1nw)(ns)(zA)(1nw)(nw)(nx圖7-11 維納濾波的信號模型和觀測信號模型)()()(nwnsnx)(nw卡爾曼濾波是根據(jù)系統(tǒng)的量測數(shù)據(jù)（即觀測數(shù)據(jù)）對系統(tǒng)的運動進行估計的，所以除了狀態(tài)方程之外，還需要量測方程。還是從維納濾波的觀測信號模型入手，圖7-11的右圖，觀測數(shù)據(jù)和信號的關(guān)系為：，一般是均值為零的高斯X(k)S(k)白誤差矢量，則量測矢量與

33、狀態(tài)矢量w(k)S(k)X(k) (7-54)上式和維納濾波的)()()(nwnsnx概念上是一致的，也就是說卡爾曼濾波的一維信號模型和維納濾波的信號模型是一致的。把式(7-54)推廣就得到更普遍的多維量測方程w(k)C(k)S(k)X(k) (7-55)C(k)X(k)S(k)上式中的稱為量測矩陣，它的引入原因是，的維數(shù)不一定與狀態(tài)矢量的維數(shù)相同，因為我們不一定能觀測到所有需要的狀態(tài)參數(shù) 量測矢量X(k)1mS(k)1nC(k)nmw(k)1m假如是的矢量，是的矢量，就是的矩陣，是的矢量。1)(kw1)A(k)S(kS(k)1w(k)C(k)S(k)X(k)S(k)C(k)1)A(k 1zw

34、(k)(k)w1X(k)1)S(k 圖7-12 卡爾曼濾波的信號模型w(k)S(k)X(k)25. 18 . 0 ,)8 . 01)(8 . 01 (36. 0)(1zzzzRss)()(mmRwwA(k)C(k)【例7-6】設(shè)卡爾曼濾波中量測方程為，已知信號的自相關(guān)函數(shù)的z變換為，噪聲的自相關(guān)函數(shù)為：，信號和噪聲統(tǒng)計獨立。求卡爾曼濾波和。信號模型中的)()()(121zAzAzRwss)8 . 01)(8 . 01 (36. 011zzzz解根據(jù)等式：可以求得：)()(8 . 01)(111zWzSzzzA)()(8 . 0) 1(1kwksks8 . 0A(k)w(k)S(k)X(k)C(

35、k)變換到時域得：因此 又因為，所以1。S(k)(k)S1)(kw1)A(k)S(kS(k)1 (7-56)w(k)C(k)S(k)X(k) (7-57)A(k)C(k)X(k)1)(kS(k)S上式中和是已知的，已知，現(xiàn)在的問題就是如何來求當(dāng)前時刻。 是觀測到的數(shù)據(jù)，也是已知的，假設(shè)信號的 上一個估計值 的估計值(k)w1w(k)S(k)(k)w1w(k)(k)S(k)X(k)S(k)X上兩式中如果沒有與，可以立即求得，估計問題的出現(xiàn)就是因為信號與噪聲的與，用上兩式和分別用和表示，得：疊加。假設(shè)暫不考慮得到的1)(kSA(k)(k)S (7-58)1)(kSC(k)A(k)(k)SC(k)(

36、k)X (7-59)X(k)(k)X必然，觀測值和估計值之間有誤差 ，它們之間的差(k)X稱為新息（innovation）：(k)XX(k)(k)X (7-60)(k)w1w(k)顯然，新息的產(chǎn)生是由于我們前面忽略了與所引起的，也就是說新息里面 (k)w1w(k)(k)XH(k)(k)w1S(k)包含了與的信息成分。因而我們用新息乘以一個修正矩陣，用它來代替式（7-56）的來對進行估計：(k)XH(k)1)(kSA(k)(k)S1)(kSC(k)A(k)H(K)X(k)1)(kSA(k) (7-61)S(k)X(k)(k)S(k)SC(k)A(k)1zX(k)1)(kSH(k)(k)X(k)X

37、由（7-56）（7-61）可以畫出卡爾曼濾波對進行估計的遞推模型，如圖7-13所示，輸入為觀測值，輸出為信號估計值。圖7-13 卡爾曼濾波的一步遞推法模型(k)SH(k)1)(kSC(k)A(k)w(k)(k)H(K)C(k)S1)(kSA(k)(k)S1)(kSC(k)A(k)w(k)1)(kw1)(kSA(k)H(K)C(k)1)(kSA(k)1H(k)w(k)1)(kw1)(kS(k)H(K)C(k)AH(k)C(k)1)I(kSA(k)1.(7-62)H(k)H(k)根據(jù)上式來求最小均方誤差下的，然后把求到的代入（7-61）則可以得 到估計值(k)S。(k)SS(k)(k)S(k)設(shè)真

38、值和估計值之間的誤差為：，誤差是個矢量，因而均方誤差是一個矩陣，用表示。把式（7-62）代入得：(k)SS(k)(k)SH(k)w(k)1)(kw1)(kS1)A(k)S(kH(K)C(k)I1.(7-63)均方誤差矩陣：(k)S(k)SE(k) (7-64)表示對向量取共軛轉(zhuǎn)置。為了計算方便，令(k)S(k)(S(k)SE(S(k)(k) (7-65) 找到和均方誤差矩陣的關(guān)系：1)(kSA(k)1)(kw1)k1)(A(k)S(kSA(k)1)(kw1)E(A(k)S(k(k)111)(k1)w(kEwA(k)1)(kS1)1)(S(k(kS1)A(k)ES(k111)Q(k1)A(k)A

39、(k)(k (7-66)(k)w1w(k)j)Q(k)(k(j)(k)wEw11把式（7-63）代入式（7-64），并且利用條件：與都是零均值的高斯白噪聲，且它們和 互不相關(guān)，協(xié)方差矩陣分別為j)R(k)(kEw(k)w(j)1)S(k)(kw111)(kS)(kw11w(k)與不相關(guān)；與及不相關(guān)。最后化簡得：(k)S(k)SE(k)k)H(k)R(k)H(H(k)C(k)1)IQ(k1)A(k)A(k)(kH(K)C(k)I. (7-67)把式（7-66）代入（7-67）得(k)k)H(k)R(k)H(H(k)C(k)(k)IH(K)C(k)IR(k)H(k)(k)C(k)H(k)C(k)H

40、(k)(k)C(k)(k)H(K)C(k)(k)SSR(k)(k)C(k)C(k)(k)C(k)U令 ，代入上式化簡： (k)H(k)H(k)SSUH(k)H(K)U(k)111)U(SH(k)S)U(SH(k)SU)U(SS(k) (7-68)01)U(SH(k)SH(k)上式第一項和第二項與修正矩陣無關(guān)，第三項是，于是可以求得最小均方誤差下的修正矩陣為：H(k)半正定矩陣，要使得均方誤差最小，則必須1R(k)(k)C(k)C(k)(k)C(k)H(k) (7-69)把上式代入(7-61)即可得均方誤差最小條件下的(k)S遞推公式。相應(yīng)的式（7-68）的第三項為零，得最小均方誤差為：(k)1

41、U)U(SS(k)(k)H(k)C(k)I(7-70)綜上所述，得到卡爾曼濾波的一步遞推公式：(k)1)Q(k1)A(k)A(k)(k (7-71)1R(k)(k)C(k)C(k)(k)C(k)H(k) (7-72)(k)(k)H(k)C(k)I (7-73)(k)S1)(kSC(k)A(k)H(K)X(k)1)(kSA(k) (7-74)(k)S(k)S(0)0(0SE)(S有了上面四個遞推公式后我們就可以得到和。如果初始狀態(tài)的統(tǒng)計特性已知，并且令)0(var()0()0()(0()0()0(SSSSSE且矩陣 R(k)C(k),Q(k),A(k),都是已知的，以及觀測量X(k)也是已知的，

42、就能用遞推 (k)S(k)0() 1 (計算法得到所有的和：將初始條件代入式（7-71）求得； ) 1 () 1 (H) 1 () 1 (H) 1 ()0(0SE)(S) 1 (H)(S 1將代入式（7-72）求得和代入式（7-73）求得；將初始條件和代入式（7-74）求得；依此類推。這樣遞推用計算機實現(xiàn) ；將非常方便。和維納濾波一樣，卡爾曼濾波也可以推廣到卡爾曼預(yù)測，推導(dǎo)過程和維納濾波到維納預(yù)測類似，也同樣有純卡爾曼預(yù)測，這里不再推導(dǎo)。w(k)S(k)X(k)25. 18 . 0 ,)8 . 01)(8 . 01 (36. 0)(1zzzzRss)()(mmRww1)0(, 0) 1(S(k

43、)S(k)(k)S(k)【例7-7】設(shè)卡爾曼濾波中量測方程為，已知信號的自相關(guān)函數(shù)的z變換為，噪聲的自相關(guān)函數(shù)為，信號和噪聲統(tǒng)計獨立，已知，在k0時刻開始觀測信號。試用卡爾曼濾波的公式求和，k0,1,2,3,4,5,6,7；以及和。穩(wěn)態(tài)時的8 . 0A(k)C(k)36. 021wQ(k)1)(var(kwR(k)解由例7-6的結(jié)果知，1，把它們代入式(7-71) (7-74)得(k)36. 064. 01)(k（1）1(k)(k)H(k)1（2）(k)(k)H(k) 1 （3） (k)S1)(kSH(K)X(k)1)(kS8 . 08 . 0（4） 1(k)(k)H(k)1(k) (k)1/

44、(k)由于是一維情況，求逆，把（1）代入（2）、（3）式，消去，再把（2）和（3）聯(lián)立，得到H(k)1.361)(k0.361)(k(k)64. 064. 0 （5） 初始條件為1)0(, 0) 1(S，k0開始觀測，利用等式（4），（5）進行遞推得：)0()0(H)0(0X)(Sk0，1.0000，1.0000，；) 1 () 1 (5 . 0)0(4 . 01XS)(S k1，0.5000，0.5000，) 1 (H)2()2(H)2(4048. 0) 1 (4762. 02XS)(Sk2，0.4048，0.4048， )3()3(H)3(3824. 0)2(4941. 03XS)(Sk3

45、，0.3824，0.3824， )4()4(H)4(3768. 0)3(4985. 04XS)(Sk4，0.3768，0.3768， )5()5(H)5(3755. 0)4(4996. 05XS)(Sk5，0.3755，0.3755， )6()6(H)6(3751. 0)5(4999. 06XS)(Sk6，0.3751，0.3751，；)7()7(H)7(3750. 0)6(5000. 07XS)(Sk7，0.3750，0.3750， 如果給定每個時刻的觀察值就可以得到每一時刻的信號估計值，上面是遞推過程，還沒有達到穩(wěn)態(tài)的情況。假設(shè)到了某一時刻k1，前后時刻的均方誤差相等，也就是誤差不再隨著遞推增加而下降，達到最小的均方誤差了，即穩(wěn)態(tài)情況，式（5）中的誤差) 1()(kk，代入（5）式可以計算到穩(wěn)態(tài)時的均方誤差為 375. 0) 1()(kk即穩(wěn)態(tài)時的修正矩陣375. 0)(kH，代入式（4）得穩(wěn)態(tài)時的信號估計： (k)SX(k)1)(kS375. 05 . 0)(zH15 . 01375. 0z化到z域有：。)(zHopt15 . 018/3z0,)5 . 0(375. 0)(nnhn