狀態(tài)最優(yōu)估計(jì)的Kalman濾波算法

狀態(tài)最優(yōu)估計(jì)的Kalman濾波算法摘要：Kalman濾波器的提出，是為了解決最優(yōu)控制無(wú)法實(shí)現(xiàn)全部狀態(tài)均用于反饋。對(duì)于那些無(wú)法反饋的狀態(tài)，則需要根據(jù)測(cè)量到的信號(hào)對(duì)其進(jìn)行估計(jì)，再作為反饋。Kalman濾波器的突破點(diǎn)在于采用狀態(tài)空間，把信號(hào)作為白噪聲作用的線性輸出，并且是將概率論與數(shù)理統(tǒng)計(jì)的新成果引用與問題的求解。本文中詳細(xì)論述了Kalman濾波器的推導(dǎo)過程和其增益計(jì)算公式，以及對(duì)其增益計(jì)算過程進(jìn)行了仿真。關(guān)鍵詞：狀態(tài)估計(jì)，Kalman濾波器，最優(yōu)控制一、引言在一定條件下，最優(yōu)控制保證系統(tǒng)是漸近穩(wěn)定的，而且具有令人滿意的性能。但是它要求全部狀態(tài)均可用于反饋，這在實(shí)際上是難以做到的。實(shí)際上常常只能測(cè)量系統(tǒng)的一部分狀態(tài)，而且在測(cè)量到的信號(hào)中還可能包含有測(cè)量噪聲。因此需首先根據(jù)量測(cè)到的信號(hào)估計(jì)出全部狀態(tài)，然后按照最優(yōu)控制規(guī)律反饋估計(jì)的狀態(tài)。對(duì)于最優(yōu)估計(jì)，Kalman給出了適合計(jì)算機(jī)計(jì)算的狀態(tài)最優(yōu)估計(jì)遞推算法，即Kalman濾波器。針對(duì)連續(xù)系統(tǒng)和離散系統(tǒng)，Kalman濾波問題又有著連續(xù)和離散Kalman濾波器的提法［1］，本文將主要討論離散模型。二、離散系統(tǒng)的Kalman濾波問題設(shè)控制對(duì)象的離散模型［2］為：(1)x(k+1)二Fx(k)+Gu(k)+v(k)y(k)二Cx(k)+w(k)(1)其中x(k)為n維狀態(tài)向量，u(k)為m維控制向量，y(k)為r維輸出向量，v(k)為n維過程干擾向量，w(k)為r維測(cè)量噪聲向量。假設(shè)v(k)和w(k)均為離散的高斯白噪聲序列，且有Ev(k)=0,Ev(k)vt(j)=V8kj(2)Ew(k)=0,Ew(k)wT(j)=W8kj其中：8kjk=8kjk=j

k豐j(3)同時(shí)設(shè)V為非負(fù)定對(duì)稱陣，W為正定對(duì)稱陣，并設(shè)v(k)和w(k)不相關(guān)。這里我們現(xiàn)將v(k)看作白噪聲序列，如果對(duì)于實(shí)際系統(tǒng)，v(k)不是白噪聲，可通過功率譜密度等將其化為標(biāo)準(zhǔn)形式，這個(gè)在這里就不再贅述。在方程(1)中存在隨機(jī)的干擾v(k)和隨機(jī)的測(cè)量噪聲w(k)，因此系統(tǒng)的狀態(tài)向量x(k)也為隨機(jī)向量，其中，y(k)是能夠量測(cè)的輸出量。問題是根據(jù)量測(cè)量y(k)估計(jì)出x(k)。若記x(k)的估計(jì)量為X(k)，則(4)x(k)=x(k)一X(k)(4)為狀態(tài)估計(jì)誤差，因而P(k)二Ex(k)XT(k) (5)為狀態(tài)估計(jì)誤差的協(xié)方差陣，顯然P(k)為非負(fù)定對(duì)稱陣。這里估計(jì)的準(zhǔn)則為根據(jù)量測(cè)量y(k),y(k-l),…最優(yōu)地估計(jì)出X(k)，以使P(k)極小〔P(k)是非負(fù)定對(duì)稱陣，因而可以比較其大小〕。這樣的估計(jì)稱為最小方差估計(jì)。根據(jù)最優(yōu)估計(jì)理論，最小方差估計(jì)為：X(k)二E[x(k)Iy(k),y(k-1),…] (6)即x(k)的最小方差估計(jì)X(k)等于在給定的直到k時(shí)刻的所有量測(cè)量y的情況下x(k)的條件期望。為了后面推導(dǎo)的方便，下面引入更一般的記號(hào)：X(jIk)二E[x(j)Iy(k),y(k-1),…] (7)根據(jù)j和k的大小關(guān)系，估計(jì)問題也可分為三類[1]：j>k 稱為預(yù)測(cè)(或外推)問題；j=k 稱為濾波問題；j<k 稱為平滑(或內(nèi)插)問題。為便于推導(dǎo)，我們引入如下記號(hào)：X(k)□X(kIk) k時(shí)刻的狀態(tài)估計(jì)X(k)=X(k)一X(k) k時(shí)刻的狀態(tài)估計(jì)誤差P(k)=EX(k)XT(k) k時(shí)刻的狀態(tài)估計(jì)誤差協(xié)方差陣X(kIk—1) 一步預(yù)報(bào)估計(jì)X(kIk一1)=X(kIk-1)一X(kIk一1) .一步預(yù)報(bào)估計(jì)誤差P(kIk-1)=EX(kIk-1)XT(kIk-1)......一步預(yù)報(bào)估計(jì)誤差協(xié)方差陣三、Kalman—步預(yù)報(bào)估計(jì)遞推公式推導(dǎo)Kalman濾波器方程，我們可以先推導(dǎo)一步預(yù)報(bào)的方程，根據(jù)式(1)和(7)得：X(k|k-1)=E[X(k)|y(k-1),y(k-2),...]TOC\o"1-5"\h\z=E{[Fx(k-1)+Gu(k-1)+v(k-1)]|y(k-1),y(k-2),???} (8)根據(jù)定義很容易求得一步預(yù)報(bào)方程：X(k|k-1)=FX(k-1)+Gu(k-1) ⑼根據(jù)式(1)和(9)，可以得到一步預(yù)報(bào)誤差：X(k|k-1)=X(k)-X(kk-1)=[FX(k-1)+Gu(k-1)+v(k-1)]-[FX(k-1)+Gu(k-1)]=FX(k—1)+v(k—1) (10)進(jìn)一步求得一步預(yù)報(bào)估計(jì)誤差的協(xié)方差陣：P(k|k-1)二Ex(klk-1)X(kIk-1)(11)二E[Fx(k-1)+v(k-1)][FX(k-1)+v(k-1)卩經(jīng)展開整理可得：P(k\k—1)=FP(k—1)FT+V (12)設(shè)x(k)的最小方差估計(jì)具有如下的形式：x(k)=x(k|k一1)+K(k)[y(k)一y(k|k一1)]=x(k|k一1)+K(k)[y(k)一Cx(x|k一1)] (13)其中K(k)稱為狀態(tài)估計(jì)器或Ka1man濾波增益矩陣。該估計(jì)器方程具有明顯的物理意義，式中第一項(xiàng)x(k|k-1)是x(k)的一步最優(yōu)預(yù)報(bào)估計(jì)，它是根據(jù)直到k-1時(shí)刻的所有量測(cè)量的信息而得到x(k)的相關(guān)的最優(yōu)估計(jì)。式中第二項(xiàng)是修正項(xiàng)，它根據(jù)最新的量測(cè)量信息y(k)來對(duì)最優(yōu)預(yù)報(bào)估計(jì)進(jìn)行修正。在第二項(xiàng)中y(k|k-1)是關(guān)于y(k)的一步預(yù)報(bào)估計(jì)，y(k|k-1)=y(k)-y(k|k-1)=y(k)-Cx(x|k-1)是關(guān)于y(k)的一步預(yù)報(bào)估計(jì)誤差，也稱新息(Innovation)。所以(13)式可以看作是一步最優(yōu)預(yù)報(bào)和新息的加權(quán)平均。問題隨之變成如何合適地選擇K(k)。為獲得x(k)的最小狀態(tài)估計(jì)，令狀態(tài)估計(jì)誤差的協(xié)方差：P(k)=Ex(k)xt(k)=E[x(k)一x(k)][x(k)一x(k)]t (14)最小?，F(xiàn)在的問題變?yōu)椋簩で驥(k)使P(k)極小?？梢宰C明，使P(k)極小等價(jià)于使如下的標(biāo)量函數(shù)：J=ExT(k)x(k) (15)極小，其中J表示x(k)的各個(gè)分量的方差之和，因而它是標(biāo)量。下面即按此準(zhǔn)則來尋求K(k)。根據(jù)式(1)和(13),可以求得x(k)的狀態(tài)估計(jì)誤差為：x(k)=x(k)一x(k)=x(k)一x(k|k一1)一K(k)E[Cx(k)+w(k)一Cx(k|k一1)]=[I―K(k)C]x(k|k―1)―K(k)w(k) (16)進(jìn)一步求得協(xié)方差陣：P(k)=Ex(k)xT(k)=E{[I—K(k)C]x(k|k—1)—K(k)w(k)}{[I—K(k)C]x(k|k—1)—K(k)w(k)}t=[I—K(k)C]P(k|k—1)[I—K(k)C]t+K(k)WK(k)t (⑺可利用攝動(dòng)法來尋找最優(yōu)的K(k)，即給K(k)一個(gè)增量AK(k)，從而P(k)也變?yōu)镻(k)+AP(k),根據(jù)上式忽略高次項(xiàng)，可以求得：AP(k)=P (k)-P(k)TOC\o"1-5"\h\zK+AK K=[I-K(k)C]P(k|k-1)(-AK(k)C)t+(-AK(k)C)P(kk-1)[I-K(k)C]t+AK(k)WKT(k)+K(k)WAKT(k)=-AK(k)Rt+RAKT(k) (18)其中：R=[I-K(k)C]P(k|k-1)Ct-K(k)W (19)如果K(k)能夠使得式(17沖的P(k)取極小值，那么，對(duì)于任意的增量AK(k)均應(yīng)有AP(k)=0。要使該點(diǎn)成立，則必須有R=[I-K(k)C]P(k|k-1)Ct-K(k)W=P(k|k-1)Ct-K(k)[CP(k|k-1)Ct+W]=0(20)即：K(k)=P(k|k-1)Ct[CP(k|k-1)Ct+W]-1 (21)最后，歸納上邊(9)(13)(21)(12)(17)即構(gòu)成了Ka1man濾波遞推公式。關(guān)鍵是Kalman增益矩陣的求解，求得增益后再利用式(9)和(13)便可以輕松的到狀態(tài)的最優(yōu)估計(jì)了。下邊重點(diǎn)對(duì)增益的計(jì)算進(jìn)行討論。四、增益矩陣計(jì)算的迭代法增益矩陣調(diào)可以直接根據(jù)式Kalman遞推公式進(jìn)行計(jì)算，下面給出迭代計(jì)算的程序流程：給定參數(shù)F,C,V,W和P(0)，給定迭代計(jì)算總步數(shù)N置k=1；按式(12)計(jì)算P(k|k-1)；按式(21)計(jì)算K(k)；按式(17)計(jì)算P(k)；如果k=N轉(zhuǎn)⑦；否則轉(zhuǎn)⑥；k=k+1，轉(zhuǎn)②；輸出K(k)和P(k)，k=1,2,…,N。在以上的計(jì)算過程中，式(5.238)還可進(jìn)一步化簡(jiǎn)，以減少計(jì)算工作量。將式(17)展開得P(k)=P(k|k-1)-K(k)CP(k|k-1)-P(k|k-1)CtKT(k)+K(k)CP(k|k-1)CtKt(k)+K(k)WK(k)t=[I-K(k)C]P(k|k-1)-P(k|k-1)CtKt(k)+K(k)[CP(k|k-1)Ct+W]K(k)t (22)式中第三項(xiàng)可由(20)式解出K(k)[CP(k|k-1)Ct+W]K(k)t=P(kk-1)CtKT(k)，于是可得：

P(k)=[I-K(k)C]P(k|k-1) (23)可見，式(23)要比式(17)簡(jiǎn)單得多，因此可在上面的程序流程中用式(23)代替式(17),這樣可以減少計(jì)算工作量。但有一點(diǎn)必須注意，P(k)是對(duì)稱陣，式(17)具有明顯的對(duì)稱的形式，因此在迭代計(jì)算過程中，即使由于存在救值計(jì)算的誤差也不致破壞P(k)的對(duì)稱性，這是利用式(17)進(jìn)行計(jì)算的優(yōu)點(diǎn)；而利用式(23)進(jìn)行計(jì)算時(shí)，雖然計(jì)算比較簡(jiǎn)單，但是由于數(shù)值計(jì)算誤差的存在，有可能破壞P(k)的對(duì)稱性，這是具體計(jì)算時(shí)應(yīng)該注意的問題。五、仿真上邊詳細(xì)敘述了Kalman濾波增益的迭代計(jì)算過程，我們下面通過實(shí)際的例子來進(jìn)行驗(yàn)證。假設(shè)某離散系統(tǒng)參數(shù)矩陣為：0.10.050.10.1同時(shí)已知v(k)和w(k)均為均值為零的白噪聲序列，且它們互不相關(guān)，v(k)和w(k)的協(xié)方差陣分別為V=Ev(k)vT(k)二p00,p二0.001,0.01,0.1W二Ew(k)wT(k)二0.110取N二40,P(0)=00，編寫MATLAB】3程序則可計(jì)算出Kalman濾波增益矩陣K(k)1K(k)2如圖K(k)1K(k)2如圖1所示。濾波增益1 濾波增益2圖1Kalman濾波增益矩陣從圖1可以看出，K(k)的各個(gè)元素隨著的增加而增加，它說明控制對(duì)象受到的干擾越大，依靠模型來進(jìn)行預(yù)報(bào)的準(zhǔn)確度便越低，從而更需要利用量測(cè)量來進(jìn)行修正。同時(shí)可以看出，K(k)是時(shí)變的增益矩陣，然而當(dāng)k增大到一定程度后，K(k)將趨于一個(gè)常數(shù)值，這正是我們所期望的。因?yàn)橐粋€(gè)定常的增益矩陣更便于計(jì)算機(jī)在線實(shí)現(xiàn)。在計(jì)算機(jī)控制系統(tǒng)中。通常要求計(jì)算的便是這個(gè)定常的增益矩陣K。利用上面給出的迭代算法，當(dāng)K(k)迭代計(jì)算到一定步數(shù)后，將收斂到定常的增益矩陣K，P(k)也將收斂到常數(shù)陣。而且可以證明，只要初始的狀態(tài)估計(jì)誤差協(xié)方差陣P(0)是非負(fù)定對(duì)稱陣，則K(k)和P(k)的穩(wěn)態(tài)值將與P(0)無(wú)關(guān)。因此若只要求計(jì)算K(k)的定常解，通?？扇(0)=0或P(O)=I。六、結(jié)論上述迭代法具有算法簡(jiǎn)單、編程容易的優(yōu)點(diǎn)，它在實(shí)際中也應(yīng)用得比較廣泛。然而對(duì)于有些情況，例如控制對(duì)象的離散模型是按照很小的采樣周期離散化而得到的，則上述迭代算法將收斂得比較慢。這時(shí)需要迭代很多次才能獲得需要的穩(wěn)態(tài)解，因而利用迭