最優(yōu)估計之卡爾曼濾波器的發(fā)散抑制方法

1、最優(yōu)估計第第7 7章章 卡爾曼濾波器的發(fā)散卡爾曼濾波器的發(fā)散抑制方法抑制方法 l 濾波的發(fā)散現(xiàn)象 l 限定增益濾波 l 誤差方差陣加權(quán)濾波 l 衰減記憶濾波 l 限定記憶濾波 l 增廣狀態(tài)濾波 l 平方根濾波 3上一章要點回顧上一章要點回顧問題：卡爾曼濾波最優(yōu)的條件？否則濾波易發(fā)散。模型精確，統(tǒng)計特性已知。4內(nèi)容提要內(nèi)容提要針對卡爾曼濾波的發(fā)散問題，討論了若干抑制濾波發(fā)散的方法。對于模型誤差導(dǎo)致的發(fā)散，可以通過直接和間接限定增益的方法增強新測量數(shù)據(jù)的作用，如限定增益濾波限定增益濾波、誤差方差陣加權(quán)誤差方差陣加權(quán)濾波濾波；可以增加新數(shù)據(jù)的比重，減小舊數(shù)據(jù)的比重，如衰減記衰減記憶濾波憶濾波和限定記

2、憶濾波限定記憶濾波；也可以將模型誤差作為狀態(tài)的一部分而估計，即增廣狀態(tài)濾波增廣狀態(tài)濾波。對于計算發(fā)散，可以采用平方根濾波法平方根濾波法，減小截斷誤差的影響。 57.1 濾波的發(fā)散現(xiàn)象濾波的發(fā)散現(xiàn)象發(fā)散：發(fā)散：實際的估計誤差超過理論預(yù)計值，非常大，甚至趨于無窮。 發(fā)散視在發(fā)散真實發(fā)散模型發(fā)散數(shù)值發(fā)散模型非完全能控能觀計算機截斷誤差系統(tǒng)模型或噪聲不準(zhǔn)兩類發(fā)散現(xiàn)象兩類發(fā)散現(xiàn)象二者區(qū)別視在發(fā)散：濾波誤差大，但有界；真實發(fā)散：濾波誤差趨于無窮。各類發(fā)散的解決方案各類發(fā)散的解決方案視在發(fā)散的解決方法：（1）如果是由模型引起的，可通過改變系統(tǒng)的結(jié)構(gòu)及參數(shù)，使系統(tǒng)的狀態(tài)完全能控能觀；（2）如果是數(shù)值發(fā)散，可以

3、采用雙字長運算，減少有效數(shù)字損失，也可以采用平方根濾波方法。 真實發(fā)散的解決方法：削弱預(yù)測在濾波估計中的作用，而增加新息的作用。 87.2 限定增益濾波限定增益濾波)(1|1|kkkkkkkkkxHzKxx來自模型來自觀測離散系統(tǒng)卡爾曼最優(yōu)估計：離散系統(tǒng)卡爾曼最優(yōu)估計：限定增益濾波的思想：限定增益濾波的思想：降低模型誤差產(chǎn)生的影響，削弱最優(yōu)估計公式中狀態(tài)預(yù)測的作用，而增加新息的作用，這需要通過限定增益來實現(xiàn)。 無關(guān)。，且與式中，測方程如下：某系統(tǒng)的狀態(tài)方程和觀0201), 0(xNvvxzkcxcxxkkkkkk例：若建立系統(tǒng)模型時忽略了常數(shù) c，即： xxkk1，由濾波基本方程得：，無驗前知

4、識，取初值0 | 00 | 00PxkkkTkkkkkkkkkkPQPPxxx| 1| 1kkPPPHRHPPkkkkkTkkkk21210 | 01212| 11211| 11111|210|kkPk時，當(dāng)1112221| 1| 11kkkRHHPHPKkTkkTkkkkkkkkzkxkkx111| 1|同理：111| 1110, 2kiikkzkxkk，以此類推，得令111111| 11| 1kkkkkkzzkxkkx1|11|1|11|1| 1111)1 (kkkkkkkkkkkkkkkkzkxkkzKxKxHzKxxkkkkvkcxvxz0真實觀測方程1101| 11122kiikkv

5、kckxxkiikkvkckxx10|121同理，kcxcxxkk01kiikkkkkvkckxxx1|121差：真實狀態(tài)與濾波值的誤kckxxETkkkk222|4) 1(真實的濾波誤差方差：）（kPkk2|。，但時，雖然當(dāng)0|TkkkkkkxxEPk濾波發(fā)散了！發(fā)散原因？建模時忽略了常數(shù)？實際上，建模時即便考慮了常數(shù)，但如果不準(zhǔn)確，即若取模型：)(1cddxxkk)(4) 1(1)(21|222|1|TkkkkTkkkkkiikkkkkxxEkkdckxxEvkdckxxx時，仍有當(dāng)則得濾波發(fā)散。分離，最終導(dǎo)致去作用，使濾波與測量作用越來越小，直至失新數(shù)據(jù)對濾波的的增長而迅速減小，使隨著使

6、增益系數(shù)而趨于零的速度過快，從的增長，發(fā)散的直接原因：隨著kKPkkkk|克服此類發(fā)散的方法：克服此類發(fā)散的方法：MkMMkkKMKkk,1,1個）之后，使（例如的減小。即在某個測量限制12) 1(222|McMxxETkkkk濾波均方誤差：發(fā)散得到抑制M 的取法：的取法：cMxxETkkkk112|M = ?原則：濾波的均方誤差（誤差方差）應(yīng)小于觀測誤差方差。147.3 誤差方差陣加權(quán)濾波誤差方差陣加權(quán)濾波 方法思想：方法思想：通過加權(quán)的方法人為地增大濾波誤差方差陣，從而間接地增間接地增大增益陣大增益陣，抑制真實發(fā)散。TkkkkkTkkkkkkkkQPsP1,11,1,1| 11,1|1.

7、人為加權(quán)法：人為加權(quán)法：倍，即提高時，人為地將計算sPPkkkk1| 11|兩種加權(quán)方法：人為加權(quán)法和自動加權(quán)法。11|0 | 01111kkkkkssRPKP，得取sKkk11lim增益不會衰減到0，發(fā)散得到抑制。倍，則有：增大上一節(jié)的例子中，若將11| 1sPkk21)1(2212110 | 022112| 22211| 111|11ssssPssPsPsPkkkkkkkkk1| 11|kkkksPP1|211|111|1|1kTkkkkkkTkkkkRHPKPHRHPP公式：用近似模型計算時采用2. 自動加權(quán)法：自動加權(quán)法：由于濾波產(chǎn)生發(fā)散的直接原因是實際估計誤差超過理論預(yù)計值 ，因新息

8、中包含了實際估計誤差的信息，可用其判斷濾波器是否發(fā)散。kkkkkTkkkkTkkkkkkkkkkkkvxHRHPHExHvxHzz1|1|1|1|tr1|kTkkkkkTkRHPH判據(jù)：kTkkkkTkkkTkkkkkTkRHPHRHPH1|1|tr1，嚴(yán)格收斂條件：當(dāng))1(tr1| kkkkTkkkTkzzE，判據(jù)：判據(jù)：當(dāng)此式不成立時，濾波發(fā)散。kTkkkkTkkRHPH1|TkkkkkTkkkkkkkkkQPSP1,11,1,1| 11,1|kTkTkkkkkkTkTkkkkkkkkTkkRHQHHPHS1,11,1,1| 11,1,1| 11,1,11,TkTkkkkkkkkTkTkk

9、kkkkTkkkHPHtrRHQHtrS1|11|1|1,11,1,1| 11,1|1| 11,1| 11,|kkkkkkkTkkkkTkkkkTkkkkkTkkkkkkkkkkkkkkkkkkkkkkPHKIPRHPHHPKQPSPxHzKxx濾波方程：濾波方程：，若估計誤差kK ，kTk ，kS，1|kkP 187.4 衰減記憶濾波衰減記憶濾波 方法思想：方法思想：是一種方差加權(quán)法方差加權(quán)法。對濾波器中的方差陣（包括系統(tǒng)噪聲方差、觀測噪聲方差、初始估計誤差方差）進(jìn)行加權(quán)，以逐漸減小舊的測量數(shù)據(jù)的比重，同時增加新數(shù)據(jù)的比重。 權(quán)值為指數(shù)函數(shù)或冪函數(shù)。1. 指數(shù)加權(quán)法：指數(shù)加權(quán)法：)0(CeC

10、權(quán)值為按以下方式加權(quán)：將11011,),0(NNNQQQRRRP1210,11110NNCCCQQeQeQeNNiiNiiNNCCCCRReReRePeNNiiNiiNii,)0(1211121110，)()()()() 1() 1,() 1() 1,()(kvkxkHkzkwkkkxkkkxNNNNN這相當(dāng)于采用新模型：klCkTNNNNNNNkiieQlwkwEkwExkvkw11)1()1(, 0)()0()()(互不相關(guān)，且和、其中，klCkTNNNNkiieRlvkvEkvE1)()(, 0)(10)0()0(Var),0( )0(NiiCNNePxxxE對模型(7.4.6)在N時刻

11、以后濾波，得- (7.4.6)濾波方程組：濾波方程組：)11() 1,()()()() 11() 1,()(kkxkkkHkzkKkkxkkkkxNNNN11)()()() 1()()() 1()(NkTNNkTNTNNRkHkkPRkHkkPkHkHkkPkK) 1,() 1,() 1,() 1| 1() 1,() 1(1kkQkkkkkkPkkkkPTNkTNN111)()()() 1()()()(kHRkHkkPkkPkHkKIkkPNkTNNNN1011)0()0()0( )0(11NiiNkiiNkiiCNNCkNkCkNkePPxxeRReQQ，其中，)11() 1,()()()(

12、) 11() 1,()(*kkxkkkHkzkKkkxkkkkx濾波方程組：濾波方程組：1*1*)()()() 1()()() 1()(kTkTTRkHkkPRkHkkPkHkHkkPkK) 1,() 1,() 1,() 1| 1() 1,() 1(1*1kkQkkekkkkPkkkkPTkCTk111*)()()1() 1()()()(kHRkHkkPkkPkHkKIkkPkT為某一時刻）（NkkxkkxkkxkkxkKkKNNN)()() 11() 11()()(*)0()0()0( )0(*PPxx，初值：11)()() 1() 1(*NkiiNkiiCNCNekkPkkPekkPkkP

13、，：令2. 冪函數(shù)加權(quán)法：冪函數(shù)加權(quán)法：)0, 1(aSSa權(quán)值為kNkNkkNkNkNNkkSQQSRRSPPQRP11001,)0(進(jìn)行加權(quán)：，將1|1,11,1,1| 11,1|11|1|1| 11,1| 11,|kkkkkkTkkkkkTkkkkkkkkkTkkkkTkkkkkkkkkkkkkkkkkPHKIPQSPPRHPHHPKxHzKxx濾波方程組：濾波方程組：00 | 00 | 000 | 0, 0PSPPxExNN初值：的影響降低了。的權(quán)減小了，即舊數(shù)據(jù)知，的作用加強了，而由在濾波中，即當(dāng)前觀測，知，說明：由*1|*1|*|*1|*1| 1kkkkkkkkkkkkkkkkkx

14、zKxHKIxzKKPPS237.5 限定記憶濾波限定記憶濾波。）：事先確定的記憶長度（影響。以前的數(shù)據(jù)對濾波值的，截斷個觀測最近的時，只利用離計算NNkzkzNkzNkzNkkkx) 1()(,),2(),1(1)|( 方法思想：方法思想：k1k1 Nk截斷Nk 個1-N)() 1()2() 1 (kzkzzz) 1|( kkxkZ111kZ)|( kkx關(guān)系圖和卡爾曼濾波基本方程中圖)|( ) 1|( 2 . 7kkxkkx卡爾曼濾波基本方程的建立過程卡爾曼濾波基本方程的建立過程: ) 1|( )1(,),2(),1 () 1 (11kkxkzzzZk)|( )(, ) 1(,),2(),

15、1 ()2(1kkxkzkzzzZk)1|( )()()() 1|( )|( ) 1|( )|( kkxkHkzkKkkxkkxkkxkkx的關(guān)系為：與限定記憶濾波思想限定記憶濾波思想: 。觀測個最近的時，只考慮離，在計算增加新觀測數(shù)據(jù)的影響的權(quán)值，程中的影響，即減小估計方為減小模型誤差對濾波)(,),2(),1()|( ) 1|( kzNkzNkzNkkkxkkx)() 1() 1()() 1 (kzkzNkzNkzz) 1|(kkxN1NN1kNkZNkNkZ1kNkZ)2() 1 ()3()|(kkxN)|(1kkxN關(guān)系圖和、限定記憶濾波中圖)|()|() 1|(3 . 71kkxkk

16、xkkxNNN*) 1|()|() 3(2*2)|()|()2(1*1) 1|()|() 1 () 1|()|(11bakkxkkxbakkxkkxbakkxkkxkkxkkxNNNNNNNN再讓二者相減，即得：兩組關(guān)系：獲得以下之間的遞推關(guān)系，首先與為獲得限定記憶濾波實現(xiàn)過程限定記憶濾波實現(xiàn)過程: 的關(guān)系式與）求（) 1|()|(11kkxkkxNN，可得：，根據(jù)卡爾曼基本方程利用觀測集kNkZ1)() 1|()()() 1|()(kTNTNRkHkkPkHkHkkPkJ)1|()()()() 1|()|(1kkxkHkzkJkkxkkxNNN) 1| 1() 1,() 1|(kkxkkkk

17、xNN1111)()() 1|() 1|()()()|(kHRkHkkPkkPkHkJIkkPkTNNN) 1,() 1| 1() 1,() 1|(kkkkPkkkkPTNN無動態(tài)噪聲的狀態(tài)方程和觀測方程： kjkTRjvkvEkvEkvkxkHkzkxkkkx)()(0)()()()()() 1() 1,()(，且其中，的關(guān)系式與）求（)|()|(21kkxkkxNN它的觀測值。的觀測值，需要先化成不是的觀測值，是，但kkNkkNkxNkxNkzNkzZZ)()()(1，觀測集)|(1kkxZNkNk，觀測集)|(1kkxZNkNk)()(),()()()()()(NkvkxkNkNkHNk

18、vNkxNkHNkz由觀測方程知：濾波方程，有的觀測值。根據(jù)卡爾曼就是將其看作觀測矩陣，則，定義kxNkzkNkNkHNk)(),()()()|(),()()()()|()|(1kkxkNkNkHNkzkJkkxkkxNNN1)(),()|(),()()(),()|()(NkTTNTTNRNkHkNkkkPkNkNkHNkHkNkkkPkJ1111),()()(),()|()|(),()()()|(kNkNkHRNkHkNkkkPkkPkNkNkHkJIkkPNkTTNNN（3）利用前兩個步驟得到 xk 的限定記憶濾波方程：)|(),()()()()|()|()1|()()()() 1|()|

19、(11kkxkNkNkHNkzkJkkxkkxkkxkHkzkJkkxkkxNNNNNN兩式相減)1| 1() 1,(),()()()()1| 1(), 1()()()() 1| 1(), 1()|(kkxkkkNkNkHNkzkKkkxkkkHkzkKkkxkkkkxNNNN)(),()()()()(),()()()(11kJkNkNkHkJIkKkJkNkNkHkJIk K其中，化為：和的定義，可將和利用前面對)()()()(kKkKkJkJ11)(),()|()()()|()(NkTTNkTNRNkHkNkkkPkKRkHkkPkK的計算公式：并將方差陣)|(kkPN),()(),()(

20、)(), 1() 1|1(), 1()|(1111kNkRNkHkNkkHRkHkkkkPkkkkPNkTTkTNTN)1| 1() 1,(),()()()()1| 1(), 1()()()() 1| 1(), 1()|(kkxkkkNkNkHNkzkKkkxkkkHkzkKkkxkkkkxNNNN加上前面已得到的估計公式：則得限定記憶的濾波公式。狀態(tài)估計公式誤差方差公式增益矩陣公式（4）初值的選取于取初值：，比較上兩式，這相當(dāng)，且即，。這相當(dāng)于取和于開始，狀態(tài)估計不依賴的影響，應(yīng)從對和為避免0)0|0( 0)0|0()0|0( )|()()|()0|0()0|0( 100 xPIPPxNNx

21、NkkkxPxNN) 1,() 1| 1() 1,() 1|()()()() 1|()|(111kkkkPkkkkPkHkRkHkkPkkPTT由動態(tài)方程可得卡爾曼濾波基本公式：kjTTTkjTTTjzjRjHkjxPkkkxkkPkjjHjRjHkjkPkkkP1110111101)()()(),()0|0( ), 0()|( )|(),()()()(),(), 0(), 0()|( )()()(),()|()|( ),()()()(),()|(11111NjTTNNNjTTNjzjRjHNjNNPNNxNjjHjRjHNjNNP327.6 增廣狀態(tài)濾波增廣狀態(tài)濾波方法思想：方法思想：將動態(tài)

22、偏差作為增廣的狀態(tài)進(jìn)行估計，解決模型誤差引起的發(fā)散問題。同時，偏差的估值還可以用于模型校正。1111, 1, 11kkkkkkkkkkkvxHzfBxx線性系統(tǒng)模型：kkkkff, 11引入增廣狀態(tài)：111kkkfxx增廣模型方程為：1111, 11kkkkkkkkvxHzxx0011, 1, 1, 1, 1kkkkkkkkkkHHB，式中：一步預(yù)報方差陣（分塊矩陣）：一步預(yù)報方差陣（分塊矩陣）：TkkkTkkkTkkkTkkkTkkkTkkkkkkkkkTkkkkkkkkkkkkkkkkkkkkkkkkkkPBPPPBPPBBPPPPBPPPPP, 14, 14, 13, 12, 12, 1

23、1, 1, 1, 1, 1, 1, 14321, 1, 1, 14| 13| 12| 11| 1| 1000估計誤差方差陣（分塊矩陣）：估計誤差方差陣（分塊矩陣）：4321|)(kkkkTkkkkTkkkkTkkkkTkkkkTkkkkkPPPPffExfEfxExxExxEP的協(xié)方差陣。：的互協(xié)方差陣；與：的互協(xié)方差陣；與：估計誤差方差陣；：kkkkTkkkkkkkkkTkkTkkkkkkkkkkkTkkkkkkkkTkkkkkfDffEPxfCxfEPfxCfxEPxPxxEP|4|3|2|1上式中，展開，有：再將 Pkk | 1TkkkkkkkkkkTkkkkkkTkkkkkkkkkkT

24、kkkkkkTkkTkkkkTkkkkkkTkkkkkkkkkDPDDBCPCBDBCBBCPPP, 1|, 14, 1| 1, 1|, 1, 1|, 12, 1| 1, 1| 1, 1, 1| 1, 1, 1| 1, 1, 11, 11, 1| 1得：由kkkkkkkkPHKPP| 111| 11| 1kkTkkkkkkkkkkkTkkkkkkkkTkkkkkkDCCPHKKDCCPDCCP| 1| 1| 1| 112111| 1| 1| 1| 11| 11| 11| 11| 10將上式展開，得kkkkkkkkkkkkkkkkkkkkkkkkDHKDDCHKCCPHKPP| 1121| 11

25、| 1| 1111| 11| 1| 1111| 11| 1，即由下式確定和其中，111| 111| 112111kTkkkkTkkkkkkRHPHHPKKK111| 1| 1| 1| 111| 1| 1| 1| 12111000kTkkkTkkkkkkkTkkkTkkkkkkkkRHDCCPHHDCCPKK111| 111| 121111| 111| 111kTkkkkTkTkkkkTkkkkTkkkkRHPHHCKRHPHHPK展開，得：增廣系統(tǒng)的卡爾曼濾波器方程： |, 1*112111|, 11| 1kkkkkkkkkkkkkkxHzKKxx|, 11|, 11111|1|, 11| 1kkkkkkkkkkkkkkkkkkkkkfBHxHzKfBxx展開得增廣系統(tǒng)的卡爾曼濾波方程：|, 11|, 11121|, 11| 1kkkkkkkkkkkkkkkkkkfBHxHzKff111| 111| 111kTkkkkTkkkkRHPHHPK其中，TkkkkkkTkkTkkkkTkkkkkkTkkkkkkkkBDBCBBCPP, 1| 1, 1, 1| 1, 1, 1| 1, 1, 1|, 1| 1kkkkkkkkPHKPP| 1111| 11| 1377.7 平方根濾波平方根濾