1、1第第7章章 卡爾曼濾波卡爾曼濾波237.1 基于新息過程的遞歸最小均方誤差估計基于新息過程的遞歸最小均方誤差估計7.1.1 標量新息過程及其性質標量新息過程及其性質1n-4( )( )( )( )H1(7.1.4)ne nd nd nz n-=-=- w z ( )( )enna定義為線性預測器在最小均方誤差意義下的預測誤差。5 從新息過程定義可知， 就是 在最小均方意義下的一步預測誤差，根據維納濾波的正交原理（估計誤差與輸入信號向量正交）， 與輸入信號向量 正交，因此， 包含了存在于當前觀測樣本中的新的信息，“新息”的含義即在于此。() () () ()()()H11|(7.1.5)nnn

2、z nd nz nz nz na-=-=-=- w zZ( )na( )z n( )na1n-z( )na( )z n6( ) ( )*E0,1,2,1n zkkna輊=-犏臌L( ) ( )*E0,1,2,1nkknaa輊=-犏臌L() ( )( )1 ,2 ,naaaL() ( )( )1 ,2 ,zzz nL序列和包含了相同的信息，即 () ( )( )() ( )( )1 ,2 ,1 ,2 ,zzz nnaaa LL 等價7()()11za=令( )( )()11221zl za=+上式兩邊乘()*1a，并取數學期望，根據新息過程的性質(2)有( )00z=()0 1|0z=Z由于，所

3、以，由 的表達式，有( )na8( ) ()( ) ()() ()*11E21E21E110zlzaaaa輊輊輊=+=犏犏犏臌臌臌( ) ()() ()( ) ()()*11*2E21E21E11E1zzlzzaaa輊輊犏犏臌臌= -= -輊輊犏臌犏臌()()( )()21223321zl zl za=+()*1a( )*2a() ( )() ()*E32E310aaaa輊輊=犏犏臌臌上式兩邊分別乘、，并取數學期望，根據，得方程組9( ) ()( ) ()( ) ()() ()( ) ( )( ) ( )( ) ( )() ( )*2122*2122E31E3121110E32E3222120

4、zl zl zzl zl zaaaaaaaaaa 輊輊=+= 犏犏臌臌 輊輊=+=犏犏臌臌 21l22l( )na可解得和的值。依此類推，可以將表示為( )( )()()()()() ()()1 11 211121 ,2nnnnnz nlz nlz nlzna-=+-+-+L()()( )( )()( )( )( )()( )( )( )( )()( )( )()()()()() ()()1121223132331 11 21111221332144321121nnnnzzl zzl zl zzl zl zl znz nlz nlz nlzaaaaa-=+=+=+=+-+-+ML10(7.1.

5、7)nnn= L za a其中() ()() ()() ()1122211112131000100101n nnnnnnnnllllll-輊犏犏犏犏=犏犏犏犏犏臌LLLLMMMOML由此可知，序列()( )( )12naaaL可由序列()( )( )12zzz nL線性表示，性質(3)得證。117.1.2 最小最小均方誤差均方誤差估計的新息過程表示估計的新息過程表示( )x n的最小均方誤差估計可表示為 ()( )H|nnx nn= wzZ根據新息過程性質(3)，上式可表示為()( )( )H1H|(7.1.8)nnnnx nnn-=wLbaaaaZ( )( )HH1nnn-=bwLna a其

6、中，為輸入時的權向量，可表示為( )()( )( )T112nnbbb n輊=臌bL12可以證明( )nbna a是輸入為時，最小均方誤差準則下的最佳權向量，即滿足維納-霍方程( ) ( )( )nnn=Abpa a( )HEnnn輊=犏臌Aa a a ana a( )( )Ennxn*輊=犏臌pa aa a其中矩陣為的自相關矩陣，為互相關向量。根據新息過程的正交性有() ( )()2EE()ijiijaaad*輊輊=-犏犏臌臌因此( )nA為對角矩陣，即( )()( )( )222diag E1,E2,Ennaaa輊輊輊=犏犏犏臌臌臌AL13容易解出權向量( )nb的各元素為()()()2,

7、1,2,Epib iiniaa=輊犏臌L()() ( )Epii xnaa*輊=犏臌( )npa ai其中，為互相關向量的第個元素。()( )() ()() ()( ) ( )()( ) ( )H1111|1|nnninnix nnbiibiibnnx nbnnaaaa*=-*-=+=-+ba aZZ14于是有迭代關系()()( ) ( )()1|1|7.1.11nnx nx nbnna*-=-+ZZ上式表明上式表明:以新息過程作為維納濾波器的輸入，若1n-()1x n-的估計值()11|nx n-Z已獲得，則可按上式的迭代方法計算出n時刻期望響應( )x n的估計值()|nx nZ，這種方法

8、帶來計算上的極大方便。157.1.3 向量新息過程及其性質向量新息過程及其性質輸入信號向量( )( )( )( )T112NNnz nznzn輊=臌zL新息過程向量( )( )( )( )T112NNnnnnaaa輊=臌a aLn( )nz()1|nn-zZ在時刻，對輸入向量的最佳線性預測向量可表示為()()()()THHH11211211|nNnnN nn-輊=犏臌zw zw zw zLZ16將標量新息過程的性質推廣到向量新息過程，為:(1)n時刻的新息過程向量( )na a和過去所有觀測向量() ( )()11 ,2 ,1nn-=-zzzLZ正交，即( ) ( )HE,1,2,1nkkna

9、 a輊=-犏臌z0L(2)n時刻的新息過程向量( )na a和過去所有新息過程向量( )ka a相互正交，即( )( )HE,1,2,1nkkna aa a輊=-犏臌0L17(3)觀測數據向量序列() ( )( )1 ,2 ,nzzzL和新息過程向量序列() ( )( )1 ,2 ,na aa aa aL另一個序列，而不丟失任何信息。即之間存在著一一對應關系，可以借助可逆線性變換從其中一個序列得到() ( )( )() ( )( )1 ,2 ,1 ,2 ,nnzzza aa aa a LL 等價187.2 系統狀態方程和觀測方程的概念系統狀態方程和觀測方程的概念一個多輸入多輸出的離散時間LTI

10、系統，可用狀態方程和輸出方程進行描述:( )( )( )()()()()()()111121111211121222212222221212111111NSNSNNNNNNNSNSNx naaabbbx nfnaaabbbxnxnfnaaabbbxnfnxn輊輊輊輊輊-犏犏犏犏犏犏犏犏犏犏-犏犏犏犏犏=+犏犏犏犏犏犏犏犏犏犏犏犏犏犏-犏犏犏犏臌臌臌臌臌LLLLMMOMMMOMMMMLL個輸入 SN個狀態變量 狀態方程狀態方程19將其表示為向量的形式，為( )()()11nnn=-+-xAxBf( )1Nnx為狀態變量 ( )1Snf為輸入向量NNA為狀態轉移矩陣N SB為輸入控制矩陣20系統輸

11、出方程系統輸出方程( )( )( )( )( )( )( )( )( )111121111211121222212222221212NSNSMMMNMMMSNSMz ncccdddx nfncccdcdznxnfnccadddxnfnzn輊輊輊輊輊犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏=+犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏犏臌臌臌臌臌LLLLMMOMMMOMMMMLLM個輸出 表示為向量形式，為( )( )( )nnn=+zCxDf( )1Mnz為輸出向量，MNC為狀態輸出矩陣，MSD為輸出控制矩陣。21對于一個受到隨機干擾的系統，系統狀態方程和輸出方程可分別表示為( )()()()1111nnn

12、n=-+-+-xAxBfv狀態方程狀態方程( )( )( )( )2nnnn=+yCxDfv系統輸出方程系統輸出方程其中，( )11Sn v( )12Mn v和分別為系統狀態噪聲和輸出噪聲，N S 為狀態噪聲輸入矩陣。對于一般的時變系統對于一般的時變系統，系統狀態方程和觀測方程可表示為以下的形式：221 狀態（過程）方程（狀態（過程）方程（state equation）( )() ()() () ()1,11,117.2.7nn nnn nn=-+-xFxvG G狀態向量：( )1Nnx；狀態轉移矩陣：(),1NNn n-F狀態噪聲輸入矩陣：(),1N Sn n-G G系統狀態噪聲：()111

13、Sn-v其中，系統的狀態轉移矩陣(),1n n-F描述了系統狀態從1n-時刻到n時刻的變化規律，它有這樣的一些特點。23(1) 乘積律乘積律() ()()1,11,1nnn nnn+-=+-FFF并可類推為() ( )(),m nn lm l=FFF(2) 求逆律求逆律()()1,m nn m-=FF且有(), n n =FI24系統狀態噪聲( )1nv通常為隨機過程向量，并假定為零均值白噪聲，其相關矩陣滿足( ) ( )( )( )1H111,E(),nknnknnkknd=輊=-=犏臌 QvvQ0上式表明，不同時刻間的狀態噪聲是統計獨立的（即白噪聲）；但并未強調同時刻不同狀態噪聲間的統計獨

14、立性，若同時刻不同狀態噪聲間也是統計獨立的，則矩陣( )1nQ是對角陣。25實際應用中，也可將噪聲輸入矩陣與狀態噪聲的乘積() ()1,11n nn-vG G看成一個向量，若仍用()11n-v表示，此時狀態方程可表示為( )() ()()1,111nn nnn=-+-xFxv(7.2.12)262 觀測（測量）方程（觀測（測量）方程（measurement equation）( )( ) ( )( )()27.2.13nnnn=+zCxv觀測向量：( )1Mnz觀測矩陣:( )MNnC觀測噪聲：( )12Mnv與狀態方程類似，觀測噪聲( )2nv通常也假定為零均值白噪聲，其相關矩陣為( ) (

15、 )( )( )2H222,E(),nknnknnkknd=輊=-=犏臌 QvvQ027 同樣，不同時刻間的觀測噪聲是統計獨立的；若同時刻不同觀測噪聲間也是統計獨立的，則矩陣( )2nQ是對角陣。 由于系統狀態噪聲和觀測噪聲是在系統的不同階段引入的，它們之間是統計獨立的，即有( )( )H12E,nkn k輊=犏臌vv0 上面給出的相關矩陣都是與時間有關的量，因此，可以用來描述非平穩的系統狀態噪聲和測量噪聲。圖7.2.1給出了系統狀態方程和觀測方程的結構。28 系統狀態方程是我們對此變化規律的一種假設。通常，狀態方程不可能和系統變化規律完全相符合。因此，已知狀態向量初始值通過遞推是無法有效提取

16、系統狀態的29 利用觀測量集合() ( )( )1 ,2 ,mzzzL對系統狀態變量( )nx進行最優估計。根據mn和之間的關系，可將問題分為以下三類：(1)mn稱為平滑問題。下面的討論將集中在卡爾曼濾波卡爾曼濾波問題上。307.3 卡爾曼濾波原理卡爾曼濾波原理 7.3.1 狀態向量的最小均方誤差估計狀態向量的最小均方誤差估計為基于觀測向量集合() ( )()1 ,2 ,1n-zzzL對i 時刻的狀態向量()ix的最小均方誤差估計，可表示為 ()( ) ( )()111|7.3.1nnikikk-=xa anZ其中，( )1Nka a 是新息過程向量。 在此系統狀態已經由標量拓展到N維向量，所

17、以( )NNik為待求的最佳權矩陣。()1|ni-xZ31時刻的狀態向量估計誤差記為i()()()1,1|ni nii-=-xxe enZ可得互相關矩陣()()()()( )( )()11E,1EEniki nlilkkl-=輊輊輊-=-犏犏犏臌臌臌xe ea aa aa aa a 由正交原理可知，在MMSE意義下，狀態向量估計誤差和新息向量是正交的，因而有()()E,1i nl輊-=犏臌0e ea a32由新息過程的統計特性可知( )()( )( )EE()nlnnnld輊輊=-犏犏臌臌a aa aa aa a由此可得( )()( )( )( )()( )( )11EEEikikkkikk-

18、輊輊輊=犏犏犏臌臌臌xxAaaaaaaaa其中( )( )( )EMMkkk輊=犏臌Aa aa a為新息過程的自相關矩陣。(7.3.3)( )ik僅與k時刻的新息( )ka a有關。33另外，根據式(7.3.1)有：()( ) ( )() ()()() ()2112|11|11nniikniikknninn-=-=+-=+-xxa aa aa annZZ令1in=-，可得()()() ()()()()() ()121121|1|111|E1111nnnnnnnnnnnnn-=-+-輊=-+-犏臌xxBxxAa aa aa annnZZZ進一步將上式表示為()()() ()121|1|11nnn

19、nnn-=-+-xxKa annZZ(7.3.6)34其中()()()()11E111N Mnnnn-輊-=-犏臌KxAa a可以看出，基于觀測向量集合() ( )()1 ,2 ,1n-zzzL對系統狀態()1n-x的估計， 可在()21|nn-xnZ的基礎上，利用新息向量()1n-a a進行修正。()1n-K其中，矩陣稱為卡爾曼增益卡爾曼增益（Kalman gain）下面將討論增益矩陣()1n-K和新息過程自相關矩陣()1n-A的計算方法。(7.3.7)357.3.2 新息過程的自相關矩陣新息過程的自相關矩陣設觀測序列的新息過程向量為( )( )()1|nnnn-=-zza anZ預測值間也

20、應滿足觀測方程(7.3.8)()( )()()1121|nnnnnnn-=+zCxvnnnZZZ(7.3.9)由于( ) ( )H2E,1,2,1nkkn輊=-犏臌0Lvz所以()21|nn-=v0nZ于是有()( )()11|nnnnn-=zCxnnZZ(7.3.11)將上式代入式(7.3.8)可得36( )( ) ( )()( )12|nnnnnn-輊=-+臌Cxxva anZ定義狀態預測誤差向量(),1n n-e e()( )()1,1|nn nnn-=-xxe enZ則新息過程又可表示為( )( ) ()( )2,1nnn nn=-+Cva ae e由于(),1n n-e e與( )2

21、nv相互獨立( )nA為( )( )() ()( )( )2E,1,1nnn nn nnn輊=-+犏臌ACCQe ee e(7.3.12)(7.3.13)(7.3.14)(7.3.15)37定義一步狀態預測誤差自相關矩陣(),1n n-P為()() (),1E,1,1N Nn nn nn n輊-=-犏臌Pe ee e則( )nA又可表示為( )( ) ()( )( )2,1nnn nnn=-+ACPCQ上式給出了新息過程自相關矩陣的表示式，但矩陣(),1n n-P待確定。(7.3.16)(7.3.17)387.3.3 卡爾曼濾波增益矩陣卡爾曼濾波增益矩陣上節給出的卡爾曼增益矩陣含有未知的系統狀

22、態因而并不能直接使用，下面來導()1n-K的求解方法。()()() () ()()2E11E111,21nnnnnnn輊輊-=-+-犏臌臌xxCva ae e(7.3.18)()()()211,21|nnnnn-=-+-xxe enZ(7.3.19)由式 ，有：( )( ) ()( )2,1nnn nn=-+Cva ae e又由式 ，可得： ()( )()1,1|nn nnn-=-xxe enZ39因此有()()()()()()()HH22E11E1,21|1,211nnnnnnnnnn-輊-犏臌輊輊=-+-+-犏臌臌xxCva aeeeenZ于是() ()() ()()()()2E111,2

23、1E1|1,21nnnnnnnnnn-輊輊-=-+-犏犏臌臌xPCxCa ae enZ此外，由正交原理可知()()2E1|1,2nnnn0-輊-=犏臌xe enZ故有()()()()E111,21nnnnn輊-=-犏臌xPCa a(7.3.20)()()()()11E111N Mnnnn-輊-=-犏臌KxAa a將上式代入可得卡爾曼濾波增益為40()()()()111,211nnnnn-=-KPCA對于n時刻，可以得到( )()( )( )1,1nn nnn-=-KPCA(7.3.21)417.3.4 卡爾曼濾波的黎卡蒂方程卡爾曼濾波的黎卡蒂方程通過上節已知，為求得卡爾曼濾波增益，需得到狀態預

24、測誤差的自相關矩陣， 本小節將介紹利用遞推的方法計算(),1n n-P定義狀態估計誤差向量()1n-e e()()()1111|nnnn-=-xxe enZ(7.3.22)定義狀態估計誤差的自相關矩陣()1n-P()() ()1E11NNnnn輊-=-犏臌Pe ee e(7.3.23)由于()111|nn-= 0vnZ可得狀態預測方程()()()11|,11|nnnn nn-=-xFxnnZZ(7.3.24)因此，狀態預測誤差可表示為()( )()() ()() ()11,1|,11,11nn nnnn nnn nn-=-=-+-xxFve eeGeGnZ求上式的自相關矩陣可得()() ()(

25、)() ()()1,1,11,1,11,1n nn nnn nn nnn n-=-+-PFPFQGGGG(7.3.25)上式稱為黎卡蒂差分方程黎卡蒂差分方程（Riccati difference equation）另外()()( ) ( )()( ) ( ) ()( )112|,1nnnnnnnnnnn nn-=+輊=+-+臌xxKxKCva ae ennnZZZ因此( )( )()()( ) ( ) ()( ) ( )2|,1,1nnnnn nnnn nnn=-=-xxKCKve ee ee enZ對上式求相關矩陣可得( )( ) ( ) ()( ) ( )( )( )( )2,1nnnn

26、nnnnnn輊輊=-臌臌+PIKCPIKCKQK(7.3.26)至此，得到了卡爾曼濾波遞推算法的所有方程。44和()0 0|xnZ( )0P(1) 計算下一時刻狀態的預測值及狀態預測誤差自相關矩陣()()()11|,11|nnnn nn-=-xFxnnZZ()() ()()() ()()1,1,11,1,11,1n nn nnn nn nnn n-=-+-PFPFQGGGG(2) 計算新息過程、新息自相關矩陣和卡爾曼增益( )( )( )()1|nnz nnn-=- Cxa anZ( )( ) ()( )( )2,1nnn nnn=-+ACPCQ( )()( )( )1,1nn nnn-=-K

27、PCA45(3) 計算下一時刻狀態估計值()()( ) ( )1|nnnnnn-=+xxKa annZZ( )( ) ( ) ()( ) ( )( )( )( )2,1nnnn nnnnnn輊輊=-臌臌+PIKCPIKCKQK(4) (1)1nn=+46卡爾曼增益和狀態估計誤差自相關矩陣另外的表達形式( )( ) ( ) ()( ) ( ) ()( ) ( )( )( )( )2,1,1nnnn nnnn nnnnnn輊=-臌輊輊=-臌臌+PIKCPIKCPIKCKQK(7.3.28)( )( )( )( )()( )( )121,1nnnnn nnn-=-KPCQPCA(7.3.27)其中，

28、式(7.3.27)和式(7.3.21)是等價的；式(7.3.28)和式(7.3.26)是等價的477.3.5 卡爾曼濾波計算步驟卡爾曼濾波計算步驟算法算法7.1 （卡爾曼濾波算法）已知條件( )() ()() ()1,11,11nn nnn nn=-+-xFxvG G( )( ) ( )( )2nnnn=+zCxv( )( )( )H111Ennn輊=犏臌vvQ( )( )( )H222Ennn輊=犏臌vvQ48初始條件()( )0 0|E0輊=臌xxZ( )( )( )( )( )0E0E00E0輊輊輊輊=-犏犏臌臌臌臌Pxxxx步驟步驟1狀態一步預測()()()111|,11|Nnnnn

29、nn-=-xFxnnZZ步驟步驟2新息過程( )( )()( )( )()111|Mnnnnnnnn-=-=-zzzCxa annZZ49步驟步驟3一步預測誤差自相關矩陣()() ()()() ()()1,1,11,1,11,1NNn nn nnn nn nnn n=-=-+-PFPFQGGGG步驟步驟4新息過程自相關矩陣( )( ) ()( )( )2,1MMnnn nnn=-+ACPCQ步驟步驟5卡爾曼增益( )()( )( )1,1nn nnn-=-KPCA( )( )( )12N Mnnn-=PCQ50步驟步驟6()()( ) ( )11|Nnnnnnn-=+xxKa annZZ步驟步

30、驟7( )( ) ( ) (),1nnnn n輊=-臌PIKCP( ) ( ) ()( ) ( ),1nnn nnn輊輊=-臌臌IKCPIKC( )( )( )2NNnnn+KQK步驟步驟851527.4 卡爾曼濾波的統計性能卡爾曼濾波的統計性能 上一節從新息過程出發推導得出了卡爾曼濾波算法， 但并未討論算法的估計結果是否滿足最小均方誤差準則。 本節中將驗證，卡爾曼濾波算法滿足最小均方誤差準則。 并將看到，從新息過程出發正是基于最小均方誤差準則， 兩者只是解決問題的出發點不同，其結論是一致的。537.4.1 卡爾曼濾波的無偏性卡爾曼濾波的無偏性由( )() ()() ()1,11,11nn n

31、nn nn=-+-xFxvG G()()( ) ( )1|nnnnnn-=+xxKa annZZ可得( )( )()() ()() ()()( ) ( )11EE|E,11,11|nnnnnn nnn nnnnn-輊輊=-臌臌=-+-xxFxvxKe eG Ga annZZ54根據()1E10n輊-=臌v()()()11|,11|nnnn nn-=-xFxnnZZ可得( )()()( )( )E,1 E1Enn nnnn輊輊輊=-臌臌臌FKe ee ea a且( )( ) ()()E,1 E1nnn nn輊輊=-臌臌CFa ae e有( )()()( ) ( ) ()()( ) ( ) ()(

32、)E,1 E1,1 E1,1 E1nn nnnnn nnnnn nn輊輊輊=-臌臌臌輊輊=-臌臌FKCFIKCFeeeeeee e55由以上遞推方程可知，為使估計誤差的均值( )( )()0E0E00|0輊輊=-=臌臌xxe enZ因此，在卡爾曼濾波算法中狀態估計的初值常設為()( )0 0|E0輊=臌xxnZ( )( )()EE|nnnn0輊輊=-=臌臌xxe enZ則則卡爾曼濾波算法所得的估計值就是無偏的。則卡爾曼濾波算法所得的估計值就是無偏的。7.4.2 卡爾曼濾波的最小均方誤差估計特性卡爾曼濾波的最小均方誤差估計特性 根據最小均方誤差準則，在n時刻，算法獲得的狀態估計均方誤差取得極小值

33、( )( )( ) ( )trtr EJ nnnn輊=犏臌Peeee( )()( ) ( ) ()( )( )( ) ( ) ()()( )( )( )( )( )2,1,1,1,1nn nnnn nnnnnn nn nnnnnn=-+-+PPKCPCKKCPPCKKQK(7.4.7) 為驗證卡爾曼濾波是否為最小均方誤差估計，將上式看成( )nK的函數，計算使均方誤差( )trnP最小的( )nK是否與卡爾曼增益矩陣具有相同的形式。57由定義可得( )( )()(),1,1nnn nn n=-=-PPPP有( )()( ) ( ) ()( )( )( )( ) ( ) ()( ) ()( )(

34、)( ) ( )( )( ) ( ) ()( ) ()( )2,1,1,1,1,1,1,1nn nnnn nnnnnnn nnn nnn nnnnnnn nnn nn輊=-+-+犏臌輊輊-臌臌輊=-+-臌輊-臌PPKCPCQKKCPCPKPKAKKCPCPK(7.4.8)對新息過程自相關矩陣進行Cholesky分解，得( )( )( )nnn=ALL同時，選取矩陣( )nV滿足( )( )( ) (),1nnnn n=-LVCP根據數學中常用的“配方法”思想，有( ) ( )( )( ) ( )( )( ) ( )( )( )( ) ( )( )( )( )( )( )( )Hnnnnnnnn

35、nnnnnnnnnn輊輊輊輊-=-犏犏臌臌臌臌輊-+犏臌KLVKLVKLLKKLVLVKVV即( ) ( )( )( ) ( )( )( ) ( )( )( ) ( ) ()( ) ()( )( )( ),1,1nnnnnnnnnnnn nnn nnnn輊輊-=臌臌輊輊-+臌臌KLVKLVKAKKCPCPKVV(7.4.9)(7.4.10)(7.4.11)59由矩陣理論知識可知( )( )( )( )( )( ) ( )( )( )( ) ( )( )( )( )( )( ) ( )( )( )( )( )( )( )( )( )( )11111nnnnnnnnnnnnnnnnnnnnnnnn

36、nn-輊輊=犏犏臌臌輊=犏臌輊=犏臌輊輊=犏犏臌臌VVVLLLLVVLLLLVVLLLLVLVLLLV將式(7.4.9)和式(7.4.10)代入上式，得( )( )()( )( ) ( ) ()1,1,1nnn nnnnn n-=-VVPCACP60進一步，將式(7.4.11)和上式分別代入(7.4.8) ，可得( )()( ) ( )( )( ) ( )( )( )( )()( ) ( )( )( ) ( )( )()( )( ) ( ) ()1,1,1,1,1nn nnnnnnnnnn nnnnnnnn nnnnn n-輊輊=-+-臌臌輊輊=-+-臌臌-PPKLVKLVVVPKLVKLVP

37、CACP利用矩陣跡的性質 trtrtr=ABAB，有( )()( ) ( )( )( ) ( )( )()( )( ) ( ) ()1trtr,1trtr,1,1nn nnnnnnnn nnnnn n-輊輊=-+-臌臌-PPKLVKLVPCACP與卡爾曼增益矩陣有關的只有第二項，選取( )nK使之為零矩陣，就可以使( )trnP最小。61因此有( ) ( )( )( )( )( )1nnnnnn-=KLVKVL0由于( )( )( )( )( )( )( )( )( )( )( )1111nnnnnnnnnnn-=輊=犏臌輊輊=犏犏臌臌KVLVLLLLVLL將式(7.4.9)和式(7.4.10

38、)代入，可得( )()( )( )1,1nn nnn-=-KPCA62 當增益滿足上式時，代價函數取得最小值。對比式(7.3.21)發現，該式與通過新息過程推導出來的卡爾曼增益矩陣是一致的，因此，卡爾曼濾波算法是滿足最小均方誤差準則的。63卡爾曼濾波在雷達目標跟蹤中的應用卡爾曼濾波在雷達目標跟蹤中的應用 當卡爾曼濾波應用于目標跟蹤時，用系統狀態方程來描述目標的運動特性，其中的狀態向量通常由目標的位置、速度和（或）加速度參量構成；觀測方程中的觀測向量則由雷達測得的目標運動參量構成。64例例 假設被跟蹤目標在二維空間中運動，它從初始位置100m100m出發，在x和y方向分別以25m/s和20m/s

39、的速度進行勻速直線運動。觀測噪聲的統計特性由x和y方向的觀測噪聲標準差描述，分別為30mxs=20mys=。試用卡爾曼濾波算法實現對該目標的跟蹤，給出目標預測和估計的位置和速度方差。其中的系統過程噪聲假設為零均值的高斯白噪聲，自相關矩陣取為22.5 I對于勻速直線運動目標，在沒有任何擾動的情況下，滿足( )()()11xx nx nT vn=-+-( )()1xxvnvn=-( )()()11yy ny nT vn=-+-( )()1yyvnvn=-若將兩方向的干擾分別記為( )xnd和( )ynd 對于勻速直線運動目標，各方向的干擾可視為相應方向的加速度。因此解解：將目標n時刻在兩方向的位置

40、分別記為( )x n和( )y n速度分別記為( )xvn和( )yvn66(7.6.4)( )()()11xxxvnvnTnd=-+-( )()()()21112yyTy ny nT vnnd=-+-+-(7.6.5)( )()()11yyyvnvnTnd=-+-(7.6.6)根據式(7.6.3) 式(7.6.6) ，可以獲得如下系統狀態方程( )() ()() ()1,11,11nn nnn nn=-+-xFxvG G(7.6.7)其中各參數分別為( )()()()21112xxTx nx nT vnnd=-+-+-(7.6.3)67狀態向量：( )( )( )( )( )Txynx nv

41、ny nvn輊=犏臌x狀態轉移矩陣：()1000100,10010001Tn nT輊犏犏犏-=犏犏犏臌F系統過程噪聲輸入矩陣：()22200,1020TTn nTT輊犏犏犏-=犏犏犏犏臌G G68系統過程噪聲：()()()T1111xynnndd輊-=-犏臌v觀測方程為( )( ) ( )( )2nnnn=+zCxv其中各參數分別為觀測向量：( )( )( )Txynznzn輊=犏臌z觀測矩陣：( )10000010n輊犏=犏臌C69過程噪聲和觀測噪聲的自相關矩陣分別為( )( )22122103002.5,01020nn輊輊犏犏=犏犏臌臌QQ 在給定系統狀態方程和觀測方程下，為進行卡爾曼濾波

42、，需給出狀態估計和估計誤差自相關矩陣的初始值，下面給出目標跟蹤時，工程上常用的狀態向量估計初始化方法。70 兩坐標雷達中，狀態向量由目標的位置和速度分量構成，即( )( )( )( )( )Txynx nvny nvn輊=犏臌x()( )( )()( )( )()T22121 2|22yyxxxyzzzzzzTT輊-犏=犏犏臌xnZ 可利用前兩個時刻的觀測值 和 來確定該向量估計的初始狀態()1z( )2z71狀態估計誤差自相關矩陣按下式進行初始化( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )( )22222222222222222222222

43、22xxxxxyxyxxxxxyxyxyxyyyyyxyxyyyyyqqTqqTqTqTqTqTqqTqqTqTqTqTqT輊犏犏犏=犏犏犏犏臌P其中，( )xxqn、( )xyqn和( )yyqn為觀測噪聲自相關矩陣的元素。( )( )( )( )( )2xxxyxyyyqnqnnqnqn輊犏=犏犏臌Q72在本例中，( )( )( )2230 ,0,20 xxxyyyqnqnqn= 若狀態向量由目標的位置、速度和加速度分量構成，即( )( )( )( )( )( )( )Txxyynx nvnany nvnan輊=犏臌x其中，( )xan和( )yan分別表示n時刻目標在兩個方向上的加速度，

44、該向量估計的初始狀態需要利用前三個時刻的觀測值來確定73()( )( )( )( )( )( )()( )( )( )( )( )( )()33323221 3|3323221xxxxxxxyyyyyyyzzzTzzzzTTTzzzTzzzzTTT輊犏犏-犏犏犏犏-犏-犏犏犏犏=犏犏犏-犏犏犏犏-犏-犏犏犏臌xnZ74估計誤差自相關矩陣按下式進行初始化( )3xxxyxyyy輊犏=犏臌PPPPP其中( )( )( )( )( )( )( )( )( )( )( )( )( )()22323433333232233223421klklklklklklklklklklklklklklklqqqTT

45、qqqqqTTTqqqqqqTTT輊犏犏犏犏+犏=犏犏犏+犏犏犏臌P75同樣，( )xxqn、( )xyqn和( )yyqn為觀測噪聲自相關矩陣( )2nQ的元素。在上述兩種情況下，濾波的迭代過程分別從3n =和4n =時刻開始。 本例中狀態向量由目標的位置和速度分量構成，因此，利用前兩個時刻的觀測值便可初始化狀態估計，76圖7.6.3(a) x方向的預測和估計位置方差77圖7.6.3(b) x方向的預測和估計速度方差78圖7.6.3(c) y方向的預測和估計位置方差79圖7.6.3(d) y方向的預測和估計速度方差80 卡爾曼濾波在數據融合中的應用卡爾曼濾波在數據融合中的應用 數據融合(da

46、ta fusion)是針對使用多個或多類傳感器的系統而開展的一種信息處理新方法。按照信息提取的層次，融合可以分成五級，包括檢測級融合、位置級融合、屬性級融合、態勢評估與威脅估計。卡爾曼濾波算法是實現多傳感器位置融合的主要技術手段之一，根據位置融合系統結構的不同，利用卡爾曼濾波理論對多傳感器數據進行最優融合有兩種途徑：集中式濾波和分布式濾波。81圖7.6.8 集中式融合系統82 集中式融合系統，融合單元可以利用所有傳感器的觀測信息進行狀態估計。i()( )()( ) ( )()( )2,1,2,iiinnnniL=+=zCxvL同時，將各傳感器的觀測方程組合在一起，用一個方程表示，可以得到如下的

47、廣義觀測方程( )( ) ( )( )2nnnn=+zCxv集中式融合系統利用一個濾波器集中處理所有觀測數據，無任何信息損失，因此，可以獲得最優估計。但隨著傳感器數目的增多，運算量也會相應增大，當傳感器數目達到一定時，將無法保證濾波器的實時性。83圖7.6.9 分布式融合系統84 圖7.6.9所示的分布式融合系統中，各個傳感器首先要進行局部濾波（與融合單元的濾波相比，各分布傳感器的濾波稱為局部濾波），并將當前的狀態估計結果送至融合單元，融合單元對各傳感器提供的局部估計進行融合給出最終的全局估計。局部濾波實質為分布傳感器的卡爾曼濾波過程，而融合單元輸出的全局估計是局部估計的線性組合，可見，融合單元在此的作用與其在集中式融合系統中的不同，它不進行卡爾曼濾波而僅實現局部估計的優化組合。85首先，考慮僅有兩個傳感器的情況。假設n時刻的局部狀態估計分別為()()()11|nnxZ和( )( )()22|nnxZ。相應