最優(yōu)估計之線性連續(xù)系統(tǒng)卡爾曼濾波

1、最優(yōu)估計第第8 8章章 線性連續(xù)系統(tǒng)線性連續(xù)系統(tǒng)卡爾曼濾波卡爾曼濾波l 離散系統(tǒng)取極限的推導方法l 卡爾曼濾波方程新息推導法l 線性連續(xù)系統(tǒng)濾波器的一般形式l 濾波的穩(wěn)定性及誤差分析研究連續(xù)系統(tǒng)的必要性研究連續(xù)系統(tǒng)的必要性：實際的物理系統(tǒng)往往是連續(xù)的，故離散系統(tǒng)的描述不能完全代替連續(xù)時間系統(tǒng)。)()()()(,0 )()(0 )()(0)()()()()()(0)()()()()(0)(00000tPtxVarttxEtt tvtxE twtxEvtwEttRvtvEtvEttQwtwEtwExxTTTTT，噪聲統(tǒng)計特性：線性連續(xù)系統(tǒng)模型：線性連續(xù)系統(tǒng)模型：)()()()()()()()()(

2、tvtxtHtztwtGtxtAtx(8.1.1)問題：問題：最小的線性估計。使，狀態(tài)估計求式，給定測量)()()() 1 . 1 . 8()()(0tXtXEPtXtttZTt48.1 離散系統(tǒng)取極限的推導方法離散系統(tǒng)取極限的推導方法推導方法思想：推導方法思想：當采樣稠密或采樣間隔趨于零時，取離散系統(tǒng)的極限，將離散系統(tǒng)的結(jié)果轉(zhuǎn)化為連續(xù)系統(tǒng)的公式。步驟步驟1：建立：建立(8.1.1)的等效離散線性系統(tǒng)數(shù)學描述的等效離散線性系統(tǒng)數(shù)學描述步驟步驟2：求等效離散模型的卡爾曼濾波方程：求等效離散模型的卡爾曼濾波方程 步驟步驟3： 對離散卡爾曼濾波公式取極限對離散卡爾曼濾波公式取極限 時當0t推導方法步

3、驟：推導方法步驟：步驟步驟1：建立：建立(8.1.1)的等效離散線性系統(tǒng)數(shù)學描述的等效離散線性系統(tǒng)數(shù)學描述)()()()()(),()(),()(ttvtxttHttztwttttxtttttxnn由 5.3 知，等效模型：tttttttAItttn)(G),()(),(其中，kjknnkjknntRVtVCovtQWtWCov)(),()(),(tjttktt00,利用離散線性系統(tǒng)卡爾曼濾波方程(132頁)及下列等效關系：tttRRttQQttKKtttPPttttPPtttPPttHHtxxtttttzztttttxxkkkkkkkkkkkkkkkkk)(,)()(),(),(),()()

4、,(),(),(),(),(11| 1|1|11,1,得等效離散線性系統(tǒng)的卡爾曼濾波方程：步驟步驟2：求等效離散模型的卡爾曼濾波方程：求等效離散模型的卡爾曼濾波方程 )( ),()()()()( ),()( txtttttHttzttKtxtttttx(8.1.3)1)()(),()()(),()(tttRttHtttPttHttHtttPttKTT(8.1.4),()(),(),(),(),(),(tttttQttttttttPttttttPTT(8.1.5),()()(),(tttPttHttKIttttP(8.1.6)步驟步驟3： 對離散卡爾曼濾波公式取極限對離散卡爾曼濾波公式取極限 時

5、當0t)( )()()()()( )()( txttAIttHttzttKtxttAIttx式，得：代入濾波方程將)3 . 1 . 8()(),(ttAItttn得：，并除以，將上式兩端同減ttx)( )( )()()()( )()( )( txtAIttHttztttKtxtAttxttx-(8.1.8) 最優(yōu)濾波方程)( )()()()( )()( txtHtztKtxtAtx取極限0t)(tK線性連續(xù)系統(tǒng)的卡爾曼濾波方程，是一個一階微分方程。11)()(),()()(),()()(),()()(),()()(ttRtttHtttPttHttHtttPttttRttHtttPttHttHt

6、ttPtttKtKTTTT取極限0t)()()()(1tRtHtPtKT- 增益矩陣- 估計誤差方差tttPttHttKtGtQtGtAttPttPtAtttPttttPTT),()()()()()()(),(),()(),(),(，得：將其代入)16. 8( ttGtQtGtAttPttPtAttPttGttQttGttAItttPttAItttPTTTT)()()()(),(),()(),()()()()(),()(),(式，得：代入，將)5 . 1 . 8()()()(),(ttGttttAItttn取極限0t)(),(),(lim0tPttPtttPt)()()()()()()()()

7、()()()()(1tPtHtRtHtPtGtQtGtAtPtPtAtPTTT黎卡提微分方程：11線性連續(xù)系統(tǒng)卡爾曼濾波求解公式線性連續(xù)系統(tǒng)卡爾曼濾波求解公式注：連續(xù)系統(tǒng)的卡爾曼濾波估計問題歸結(jié)為求解微分方程問題； 矩陣黎卡提微分方程很難求解。)()()()(1tRtHtPtKT濾波增益方程：濾波增益方程：)()()()()()()()()()()()()(1tPtHtRtHtPtFtQtFtAtPtPtAtPTTT濾波誤差方差矩陣黎卡提方程：濾波誤差方差矩陣黎卡提方程：)( )()()()( )()( txtHtztKtxtAtx最優(yōu)濾波方程：最優(yōu)濾波方程：12線性連續(xù)系統(tǒng)卡爾曼濾波方程線性

8、連續(xù)系統(tǒng)卡爾曼濾波方程13兩點說明：兩點說明：是線性最小方差估計。即條件下的均值在是、ttttZtXEttXttZZtXtX00| )()|(,),()()(100)()()()(0)()()()(2tZtZEtZtZEtXtXEtZtXETTTT即也正交于估計量，估計誤差正交于測量量正交投影性質(zhì)，由線性最小方差估計的、框圖如下：線性連續(xù)系統(tǒng))()()()()()()()()(tvtxtHtztwtGtxtAtx)(tG)(tx)(tv)(tw)(0txs1)(tH)(tA+)(tz結(jié)構(gòu)圖如下：作用下的線性系統(tǒng)，其可視為一個濾波方程：)()()()()( )()( tztKtztKtxtAtx

9、)(tz+)(tA)(tK)( 0tx)(tH)( tx)( tzs1+168.2 卡爾曼濾波方程新息推導法卡爾曼濾波方程新息推導法新息的性質(zhì)：新息的性質(zhì)：新息是一個與測量噪聲有相同統(tǒng)計值的白噪聲過程。)()()()()()()()()(tvtxtHtztwtGtxtAtx系統(tǒng)模型：系統(tǒng)模型：新息：新息：的新成份。新息中包含為新息過程。定義方差估計，最小得到的區(qū)間的在為由設)()( )()()()()()( 00tztxtHtztztXZtttztxtt17推導過程推導過程步驟步驟1：構(gòu)造估計量的函數(shù)形式：構(gòu)造估計量的函數(shù)形式dzttxtztxtt)(),()( )()( 0*的線性函數(shù)：是假

10、定的最小方差估計。以得到，選擇)(),(*txt)(),()()(),()()(),()()( *00sRstdsRtdszzEtsztxEttTttT)()()()( sxtxEsztxETT估計與測量的正交性)()()(),(1*sRsztxEtTttTdzRztxEtx0)()()()()( 1步驟步驟2：對上述函數(shù)關于時間求導：對上述函數(shù)關于時間求導ttTTdzRztxEtztRtztxEtx0)()()()()()()()()( 11ttTttTdzRztwEtGdzRztxEtAtztK00)()()()()()()()()()()()(11)()()()(tZtKtXtA)()(

11、)()()()(tXtHtZtKtXtA)()()()()()(tZtKtXtHtKtA步驟步驟3：確定增益陣：確定增益陣 K(t)()()()(1tRtztxEtKT)()()()()( )()()()()()()( )()()(tHtPtHtxtxEtHtxtxEtvtxtHtxtxEtztxETTTTTTT)()()()(1tRtHtPtKT步驟步驟4：求：求 P(t) 的導數(shù)的導數(shù))()()(txtxEtPT0)()()()(0)()()()(tztzEtztzEtxtxEtztxETTTT)()()()(1tRtHtPtKT)()()()()()()()()()()()()(1tGt

12、QtGtPtHtRtHtPtAtPtPtAtPTTT與極限推導法的結(jié)果一致。)()()()()()()()()()()()()(1tGtQtGtPtHtRtHtPtAtPtPtAtPTTT21連續(xù)線性定常系統(tǒng)的卡爾曼濾波連續(xù)線性定常系統(tǒng)的卡爾曼濾波)()()()()()(tvtHxtztGwtAxtx若系統(tǒng)模型中的各參數(shù)為常數(shù)，即)()()()()()()()()()()()()(1tGtQtGtPtHtRtHtPtAtPtPtAtPTTT當估計過程達到穩(wěn)態(tài)時，黎卡提微分方程中的與時間無關，其微分為零，則TTTGQGHPRPHPAAP101RHPKPT：差的穩(wěn)態(tài)值，則增益為即為卡爾曼濾波誤差方

13、其解)( )()( )( txHtzKtxAtx濾波方程為：例)()(01)()(10)(0010)(tvtxtztwtxtx二階系統(tǒng)狀態(tài)及觀測方程：0001)0(var0)0()(2)()(cov)(4)()(cov0PxExtvtvtwtw，噪聲及初值：求卡爾曼濾波方程及增益、方差矩陣方程。 00010)0(2)(4)(01)(10)(0010)(0PxtRtQtHtGtA，解 由已知：10104)(0110)(210100)()(0010)(10)(21)()( 01 )()()( 0010)( tPtPtPtPtPtPtKtxtztKtxtx由濾波公式，得：)()()()()()()(

14、)()()(2221121122211211tPtPtPtPtPtPtPtPtPtP，定義：)(214)()(21)()()(21)()(21)(2)()()()()(2121211221211222111222211211tPtPtPtPtPtPtPtPtPtPtPtPtPtP則：0)0()(214)(0)0()()()(21)()(1)0()(21)(2)(2221222122112112212112111211PtPtPPtPtPtPtPtPPtPtPtP，將上式展開，有：解此非線性聯(lián)立微分方程組，求得方差 P(t)后，得增益矩陣：)( 01 )(2)(2)()( 0010)( 1211

15、txtztPtPtxtx2)(2)(01)()()()(21)(121122211211tPtPtPtPtPtPtK和濾波方程：258.3 線性連續(xù)系統(tǒng)卡爾曼濾波器的一般形式線性連續(xù)系統(tǒng)卡爾曼濾波器的一般形式系統(tǒng)模型：系統(tǒng)模型：)()()()()()()()()()()(tvtxtHtztwtGtutBtxtAtxw(t) 和 v(t) 均為零均值白噪聲過程，且)()()()()()()()()()()()(ttSvtwEttRvtvEttQwtwETTT建立與式(8.3.1)(8.3.2)等效的線性離散系統(tǒng)的數(shù)學模型： )()()()()(),()(),()(),()(ttvttxttHtt

16、ztwttttuttttxtttttxnn- (8.3.1)- (8.3.2)建立與式(8.3.1)(8.3.2)等效的線性離散系統(tǒng)的數(shù)學模型： )()()()()(),()(),()(),()(ttvttxttHttztwttttuttttxtttttxnn含控制項，過程噪聲和觀測噪聲相關。)()(),()()(),()()(),(tOttGttttOttBttttOttAIttt 是零均值分段常值白噪聲過程，其協(xié)方差陣分別為： , 2, 1, 0,),(),(0ktktttvtwnnkjnnkjnnkjnnttSvtwttRvtvttQwtw)()(),(cov)()(),(cov)()(

17、),(cov式中：將下列代換關系： 帶入離散卡爾曼濾波公式(6.3.4a)(6.3.4e)，得： )/( )()()()|( )( tttxttHttzttKtttxttx(8.3.8a)( )()()()(),()( ),()|( txtHtztJtuttttxttttttx(8.3.8b)()(),()()(),()(11tRtStttttRttSttttJ(8.3.8c)1)()(),()()(),()(tttRttHtttPttHttHtttPttKTT(8.3.8d)()()(),()(),()()(),(),()()(),(),(tJttRtJtttttQttttHtJtttttP

18、tHtJttttttPTTT(8.3.8e),()()()(tttPttHttKIttP(8.3.8f)(8.3.8d)()()()()(),()()(),(lim)(lim)(1100tRtHtPttttRtttHtttPttHttHtttPtttKtKTTTtt(8.3.9)( )()()()()()( )()( )( lim)( 0txtHtztKtutBtxtAttxttxtxst(8.3.10)()()()()()()()()()(11tRtStGtHtPtRtStGtKtKTs(8.3.11)(8.3.8d)(8.3.8a)(8.3.6)、(8.3.8b) )( )()()()()

19、()()()( )()( )()()()( )()()()()()()()( )()( )( 11ttxtHtztRtStGtutBtxtAtxttHttzttKttxtHtztRtStGtutBtxtAtxttx(8.3.8a)(8.3.6)、(8.3.8b) (8.3.8e)(8.3.6)、(8.3.8c) (8.3.15)ttGtStRtStGtGtQtGtGtStRtHttPttPtHtRtStGtAttPttPtAttPtttPTTTTT)()()()()()()()()()()()(),(),()()()()()(),(),()(),(),(111)()()()()()()()()

20、()()()()()()()()()()()()()()()()()()()(lim1110tPtHtKtGtQtGtGtStRtStGtGtStRtHtPtPtHtRtStGtAtPtPtAtPttPttPTTTTTt)()()()()()()()()()()(1tKtRtKtGtQtGtAtPtPtAtPTssTT(8.3.9)(8.3.11)(8.3.8f)31線性連續(xù)系統(tǒng)卡爾曼濾波的一般形式：線性連續(xù)系統(tǒng)卡爾曼濾波的一般形式：)( )()()()()()( )()( txtHtztKtutBtxtAtxs(8.3.16a)()()()()()(1tRtStGtHtPtKTs(8.3.1

21、6b)()()()()()()()()()()(1tKtRtKtGtQtGtAtPtPtAtPTssTT(8.3.16c)328.4 濾波的穩(wěn)定性及誤差分析濾波的穩(wěn)定性及誤差分析8.4.1濾波器的穩(wěn)定性濾波器的穩(wěn)定性)()()()()()()()()(tvtxtHtztwtGtxtAtx系統(tǒng)模型：)()()( )()()()( tztKtxtHtKtAtx濾波方程：)()()( )()( tztKtxtAtx)()()()(tHtKtAtA)( )()( txtAtx研究濾波的穩(wěn)定性，只需研究濾波方程對應齊方程的穩(wěn)定性。連續(xù)線性系統(tǒng)的能控能觀性：連續(xù)線性系統(tǒng)的能控能觀性：控的。則系統(tǒng)的狀態(tài)時完

22、全能能控性矩陣滿足：若對于任意初始時刻0),()()()(),(),(,000ttTTCdtGQGtttWt則系統(tǒng)一致完全能控。為任意時刻），有：（，若存在IttWItttC1010011),(00。則系統(tǒng)的狀態(tài)完全能觀，能觀性矩陣滿足：若對任意初始時刻0),()()()(),(),(0100ttTTOdtHRHtttWt的。則系統(tǒng)是一致完全能觀，有：，對，任意時刻，若存在IttWItttO2020022),(00濾波穩(wěn)定性定理濾波穩(wěn)定性定理 。時，且當，成立，使得對所有的，進穩(wěn)定，即存在，且在大范圍內(nèi)一致漸最優(yōu)濾波一致漸進穩(wěn)定能觀的，則其致完全能控和一致完全如果線性連續(xù)系統(tǒng)為一0),(),(0000)(2002101tttecttttccttc穩(wěn)定性定理表明，當測量時間足夠長，濾波系統(tǒng)的最優(yōu)濾波值最終與初始狀態(tài)如何選取無關?？梢宰C明，濾波估計誤差的方差也將最終與初始誤差方差陣的選取無關，而趨于穩(wěn)態(tài)值。濾波增益矩陣也具有這種漸進特性。 例 系統(tǒng)的狀態(tài)方程和觀測方程如下： )()()()(00)()()(10)()(0010)()(212121212121tvtvt