帶非線性等式約束無跡卡爾曼濾波方法

湯 啟，何臘梅

(四川大學(xué) 數(shù)學(xué)學(xué)院，成都610064)

(*通信作者電子郵箱helm111@163．com)

0 引言

動(dòng)態(tài)隨機(jī)系統(tǒng)的狀態(tài)估計(jì)在包括自動(dòng)化控制、信號(hào)處理、航行、目標(biāo)追蹤及計(jì)量經(jīng)濟(jì)學(xué)等領(lǐng)域中極其重要［1］。狀態(tài)估計(jì)的目的在于依據(jù)由狀態(tài)轉(zhuǎn)移方程及由觀測(cè)方程描述出的狀態(tài)空間模型，找到最優(yōu)或次優(yōu)的估計(jì)。在線性系統(tǒng)且噪聲是高斯分布的情形時(shí)，Kalman濾波是最優(yōu)的狀態(tài)估計(jì)器，而對(duì)于其他的情形，最優(yōu)的狀態(tài)估計(jì)器卻不容易構(gòu)造得到。對(duì)于非線性的系統(tǒng)，許多次優(yōu)的估計(jì)技術(shù)被提出，如擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)、無跡卡爾曼濾波(Unscented Kalman Filter，UKF)、滾動(dòng)時(shí)域估計(jì)(Moving Horizon Estimation，MHE)及粒子濾波(Particle Filter，PF)等。

實(shí)際問題中，很多動(dòng)態(tài)系統(tǒng)都滿足額外的條件信息，即狀態(tài)要滿足一些約束條件，這些約束源自于物理規(guī)律、數(shù)學(xué)條件及一些實(shí)際限制條件等，并能用一系列線性或非線性的等式或不等式予以描述。正確地吸納約束信息到濾波過程中，有助于提高狀態(tài)估計(jì)的質(zhì)量和精確度;然而，直接處理約束，尤其是非線性情形下的非線性約束，卻并不是十分簡(jiǎn)單。

對(duì)于線性約束情形下的狀態(tài)估計(jì)，有大量現(xiàn)有的方法可以使用，如投影法、偽觀測(cè)法等。對(duì)于帶約束的非線性系統(tǒng)狀態(tài)估計(jì)問題，一般都是基于前面提到的非線性濾波方法并結(jié)合這些約束處理手段構(gòu)造約束估計(jì)算法。

UKF因其自身的簡(jiǎn)單性及有效性，自從被提出后就被廣泛地研究和應(yīng)用，關(guān)于這方面的研究成果頗豐。經(jīng)過20多年的發(fā)展，UKF方法日趨完善，當(dāng)前關(guān)于UKF的研究方向主要在于sigma點(diǎn)結(jié)構(gòu)調(diào)整和構(gòu)建更高階或更穩(wěn)健UKF兩方面，旨在提升UKF的估計(jì)質(zhì)量和計(jì)算效率:前者主要通過調(diào)整sigma點(diǎn)的權(quán)值從而構(gòu)造非對(duì)稱的sigma散布［2－3］以及通過優(yōu)化手段調(diào)整無跡變換(Unscented Transform，UT)中的參數(shù)與協(xié)差陣分解方式以構(gòu)建自適應(yīng)的UKF［4－5］;后者包括通過再引入自由參數(shù)以構(gòu)建更高階形式的UKF［6］和通過結(jié)合其他一些手段(如二次規(guī)劃、非線性規(guī)劃等優(yōu)化方法，粒子濾波)以消解UKF的缺點(diǎn)從而構(gòu)建更穩(wěn)健的濾波［7－8］。在UKF框架下，結(jié)合一些約束手段，如別針法、優(yōu)化法、約束同化法、軟約束近似等，各種各樣的約束無跡卡爾曼濾波算法被提出［9－11］，這些約束UKF濾波算法都通過對(duì)UKF估計(jì)進(jìn)行再處理以獲得關(guān)于狀態(tài)的約束估計(jì);不少學(xué)者也提出在無跡變換階段時(shí)，就將約束信息考慮進(jìn)UKF算法過程從而直接得到約束估計(jì)，例如，針對(duì)區(qū)間約束的無跡變換形式(Interval Constrained Unscented Transform，ICUT)［12］，針對(duì)線性不等式約束的無跡變換(Linear Constrained Unscented Transform，LCUT)［2，13］以及基于內(nèi)點(diǎn)法構(gòu)造的無跡卡爾曼濾波［14］。

偽觀測(cè)法把約束視作額外的觀測(cè)，因其簡(jiǎn)便直接而成為處理帶等式約束狀態(tài)估計(jì)問題的主要方法之一。批處理方法和序貫處理方法是偽觀測(cè)法中常見的手段，這兩種方法都能用于構(gòu)造線性最小均方差(Linear Minimum Mean-Squared Error，LMMSE)估計(jì)器［15］，前者直接利用約束向量對(duì)觀測(cè)進(jìn)行擴(kuò)維，而后者通過對(duì)觀測(cè)及約束的兩次更新階段而實(shí)現(xiàn)對(duì)約束以及觀測(cè)的吸納。然而，批處理LMMSE估計(jì)器在實(shí)際應(yīng)用中可能遭遇奇異性以及高計(jì)算載荷的問題。序貫處理作為一種經(jīng)典的手段能夠避免奇異性的問題，因其遞歸地調(diào)用序貫LMMSE估計(jì)器從而能夠減輕計(jì)算負(fù)擔(dān)。在序貫處理方法中，考慮觀測(cè)與約束的處理次序問題，文獻(xiàn)［15］給出兩種形式的序貫LMMSE估計(jì)器。在線性情形下，這兩種形式的估計(jì)器有相同的計(jì)算復(fù)雜度且都等價(jià)于批處理LMMSE估計(jì)器。但是，在非線性情形下，觀測(cè)與約束的處理次序會(huì)影響到估計(jì)效果，兩種方法的等價(jià)性不再成立。接下來，很自然地就會(huì)考慮在序貫方法中如何去決定處理次序以求獲得較好的估計(jì)結(jié)果的問題。

在序貫處理中，約束與觀測(cè)孰先孰后進(jìn)行處理的問題一直莫衷一是，有學(xué)者提出通過度量觀測(cè)方程和約束方程的非線性程度來決定優(yōu)先性［16－17］。但從估計(jì)融合的角度來看，觀測(cè)和約束可看作是由兩個(gè)傳感器觀測(cè)得到，序貫處理過程實(shí)際是傳感器估計(jì)結(jié)果的融合過程，而傳感器之間的傳遞方向會(huì)影響融合后的估計(jì)結(jié)果［18］，低信噪比的傳感器向較高信噪比傳感器的傳輸方向相較于與之相對(duì)的傳輸方向會(huì)得到更好的估計(jì)結(jié)果，而這可能與前面依據(jù)非線性度量來確定次序的方法產(chǎn)生不一致的結(jié)論。為了解決這一問題，提出另外一種基于UKF的序貫估計(jì)方法。在該方法中，整個(gè)系統(tǒng)被虛構(gòu)地分離為兩套子系統(tǒng)，而兩套子系統(tǒng)中的觀測(cè)方程分別是實(shí)際的觀測(cè)方程及約束方程，狀態(tài)估計(jì)的結(jié)果則由交互并行使用這兩個(gè)子系統(tǒng)而形成的兩條濾波鏈接替給出。由于避免去事先確定二者的處理優(yōu)先次序，且獲得的估計(jì)效果優(yōu)于序貫估計(jì)器，所以能解決序貫估計(jì)方法中處理次序?qū)烙?jì)結(jié)果的影響這一問題。

1 動(dòng)態(tài)系統(tǒng)及約束描述

一般形式的離散時(shí)間非線性動(dòng)態(tài)系統(tǒng)為:

其中:xk∈Rnx表示系統(tǒng)狀態(tài)(向量)，yk∈Rny表示帶噪聲的觀測(cè)(向量)，nx、ny分別表示狀態(tài)及觀測(cè)向量的維數(shù);wk∈Rnx是過程噪聲，vk∈Rny是觀測(cè)噪聲，協(xié)差陣分別是Qk和Rk，f(·)和 h(·)都是非線性函數(shù)。非線性約束方程為:

其中:c(·)是非線性的約束函數(shù)，dk是約束向量。此外，約束也

可以通過可行域的形式給出:

2 無跡卡爾曼濾波

2．1 無跡變換

不同于在EKF中對(duì)式(1)及式(2)進(jìn)行線性化而后再使用標(biāo)準(zhǔn)Kalman濾波公式，UKF利用無跡變換來獲得對(duì)狀態(tài)的估計(jì)。在已知隨機(jī)變量sk的估計(jì)^sk與估計(jì)誤差協(xié)差陣協(xié)差陣Pk，以及非線性變換zk=g(sk)時(shí)，利用無跡變換可以得到關(guān)于隨機(jī)變量zk的均值及協(xié)差陣的近似，即^zk和Pzk。

無跡變換是建立在一系列確定選擇出的sigma點(diǎn) χk，i(i=0，1，2，…，2nx) 之上的，這些 sigma 點(diǎn)滿足如下條件:其中:仿真實(shí)驗(yàn)中無跡變換里面的參數(shù)α、β、κ的取值參照其他相關(guān)文獻(xiàn)的數(shù)值設(shè)定，設(shè)置為α=1，β=2，κ=0，后面不再贅述。

對(duì)得到的各個(gè) sigma點(diǎn)應(yīng)用非線性變換 g(·)，得到γk，i=g(χk，i)(i=0，1，2，…，2nx)，而關(guān)于隨機(jī)變量 zk的估計(jì)結(jié)果即是:

2．2 無跡卡爾曼濾波算法

類似于Kalman濾波過程，UKF也是一種兩階段估計(jì)器，即過程更新及觀測(cè)更新兩階段。過程更新階段的步驟包括:

其中: χk－1|k－1指先驗(yàn) sigma 矩陣，χk－1|k－1，i指先驗(yàn) sigma 點(diǎn)，χk|k－1，i是衍化 sigma 點(diǎn)，^xk|k－1與 Pk|k－1分別是先驗(yàn)估計(jì)、先驗(yàn)估計(jì)誤差協(xié)差陣，γk|k－1，i是觀測(cè)衍化 sigma 點(diǎn)，^yk|k－1是觀測(cè)預(yù)測(cè)值，Pykyk是觀測(cè)變量協(xié)差陣，Pxkyk是交互協(xié)差陣。

觀測(cè)更新階段的過程步驟有

其中:Kk表示增益矩陣，^xk|k與Pk|k分別指后驗(yàn)估計(jì)、后驗(yàn)估計(jì)誤差協(xié)差陣。

值得注意的是式(14)未必一定需要，由式(14)得到的衍化sigma點(diǎn)可以由式(11)中的先驗(yàn)sigma點(diǎn)予以取代，這可以節(jié)省計(jì)算量但代價(jià)是犧牲一定程度的估計(jì)性能［20］。

此外，還有另一種等價(jià)形式的觀測(cè)更新算法步驟過程在文獻(xiàn)［21］中給出，被稱作校正數(shù)據(jù)同化階段，其計(jì)算過程如下:

其中，χk|k，i'是校正 sigma 點(diǎn)，而 χk|k－1，i、Kk、γk|k－1，i分別由式(11)或式(14)、式(19)、式(15)給出。

2．3 約束處理方式

如前所述，許多線性情形下所用到的約束處理手段可以通過與UKF或者UT結(jié)合構(gòu)造出不同種類的約束無跡卡爾曼濾波算法。下面將介紹基于UKF及偽觀測(cè)法、約束同化法構(gòu)建的約束算法。

2．3．1 偽觀測(cè)法

偽觀測(cè)法適用于等式約束的情形，也被稱作是完美觀測(cè)(Perfect Measurement，PM)法，其把約束看作是額外的無噪聲的觀測(cè)，從而可以仿照觀測(cè)更新步驟吸納約束信息，所以各種各樣的帶有預(yù)測(cè)校正形式的濾波方法，如EKF、UKF等，都能與這種方法相結(jié)合從而構(gòu)造出狀態(tài)約束濾波算法。批處理與序貫處理是偽觀測(cè)法中常用的用以構(gòu)造LMMSE估計(jì)器的手段，在批處理LMMSE估計(jì)器中，將約束與觀測(cè)進(jìn)行合并擴(kuò)維，擴(kuò)維后的觀測(cè)向量形式為:

以式(25)重新描述觀測(cè)方程，得到新的系統(tǒng)模型:

注意到重寫過后的系統(tǒng)方程不再帶有約束，直接應(yīng)用前面介紹的UKF算法過程就可以得到約束狀態(tài)估計(jì)與，只需要將式(10) 中的分別替換成

盡管批處理LMMSE估計(jì)器直觀且簡(jiǎn)潔，但由于擴(kuò)維后的觀測(cè)噪聲協(xié)差陣是奇異的，在求式(19)中的增益矩陣時(shí)可能遭遇數(shù)值問題，而序貫處理是應(yīng)對(duì)這一問題的一種方式。

在使用批處理手段時(shí)，除了上面的結(jié)合UKF的方法之外，也可以使用線性化的方法，即對(duì)重構(gòu)出的系統(tǒng)，即式(26)與式(27)，使用EKF算法過程即能構(gòu)造出批處理EKF算法。

2．3．2 約束同化法

在約束同化法［22］中，約束被當(dāng)作觀測(cè)來進(jìn)行處理，并通過另外的類似于觀測(cè)更新階段的過程，即約束同化階段，吸納約束信息。約束同化階段的算法步驟如下，

其中:ζk，i是約束衍化sigma點(diǎn)，^xk|k與Pk|k由UKF算法過程得到，而d^

k，Pdkdk及Pxkdk由下式得到:

其中，式(32)中加入正定矩陣δInd是用來應(yīng)對(duì)矩陣Pdkdk可能奇異時(shí)的情形，10－15≤δ≤10－9。而約束增益矩陣，約束估計(jì)x^ck|k及約束估計(jì)誤差協(xié)差陣Pck|k的計(jì)算式分別為:

得到的約束估計(jì)接下來參與到下一時(shí)刻k+1的過程更新階段式(10)中，即構(gòu)成帶反饋的濾波過程。

觀測(cè)更新階段與約束同化階段各自得到的估計(jì)結(jié)果之間不同的傳遞方向，即產(chǎn)生了文獻(xiàn)［15］中所給出的兩種不同形式的序貫LMMSE估計(jì)器方法，即Form 1及Form 2兩種序貫形式的LMMSE估計(jì)器。對(duì)于觀測(cè)及約束的先后處理次序的不同，會(huì)造成Form 1及Form 2估計(jì)結(jié)果有差異。關(guān)于這二者處理優(yōu)先性的觀點(diǎn)一直沒有定論。接下來，基于UKF算法，給出一種新的方法，該方法無需事先確定處理次序，但相對(duì)于Form 1與Form 2形式的UKF算法能獲得更好的估計(jì)結(jié)果。

3 并行無跡濾波

3．1 子系統(tǒng)方程

關(guān)于動(dòng)態(tài)系統(tǒng)及約束方程已在式(1)～(3)中進(jìn)行了定義，在提出的并行無跡濾波(Sub-system Parallel Unscented Kalman Filter，SPUKF)中，將原動(dòng)態(tài)系統(tǒng)虛擬地分離成兩個(gè)子系統(tǒng)，分別記作System 1和 System 2，它們分別由過程方程各自與觀測(cè)方程以及約束方程組合而得到，即:

3．2 SPUKF 算法過程

SPUKF算法由兩套濾波更新過程 (即濾波鏈)所組成，分別記作成Process One和 Process Two。這兩條濾波過程是由System 1及System 2不同的先后調(diào)用次序而得到，即兩條濾波過程各自對(duì)應(yīng)于序貫LMMSE估計(jì)器的兩套形式，F(xiàn)orm 1與Form 2，而每套濾波過程得到的是間隔兩個(gè)采樣時(shí)間間隙2T的時(shí)刻的狀態(tài)估計(jì)。Process One和Process Two所獲得的估計(jì)相組合即得到SPUKF方法的估計(jì)結(jié)果，顯然地，只考慮其中某一套過程鏈不能得到狀態(tài)在各個(gè)時(shí)刻的估計(jì)結(jié)果，所以需要將該兩種濾波鏈進(jìn)行齒合才能得到完整的各個(gè)時(shí)刻的估計(jì)，就像用拉鏈將兩條鏈帶銜接起來一樣，SPUKF算法流程圖如圖1所示。3．2．1 Process One

圖1 SPUKF算法流程Fig．1 Flow chart of SPUKF

在Process One中，System 2先于System 1被調(diào)用，即通過System 2得到的更新過后的結(jié)果將會(huì)作為下一時(shí)刻的先驗(yàn)估計(jì)向System 1傳遞，之后接著將得到的估計(jì)返送回System 2，設(shè)定System 1更新完成后的結(jié)果作為Process One的估計(jì)，并設(shè)定 Process One 的初始值為 x0、P0。令 k=1，2，3，…，Process One的具體算法步驟如下:

對(duì)于 k=1，2，…

1)生成sigma點(diǎn)。利用System 1執(zhí)行完成過后的估計(jì)即 Process One 估計(jì)，進(jìn)行 UT 變換生成先驗(yàn)sigma點(diǎn):

2)處理System 2時(shí)的更新步驟。接下來處理System 2，以獲得該系統(tǒng)在時(shí)刻k的狀態(tài)估計(jì)珘xk|k及珘Pk|k:

類似地，式(46)～(48)也可以做這樣的替換。

從上面的計(jì)算過程可以看到，在Process One濾波鏈中，由k－1時(shí)刻的估計(jì)推更新得到了 k+1時(shí)刻的估計(jì)，又因?yàn)槌跏贾翟O(shè)置為x0與P0，這說明利用Process One濾波鏈能得到狀態(tài)向量在偶數(shù)時(shí)刻的估計(jì)。為了補(bǔ)齊狀態(tài)在奇數(shù)時(shí)刻的估計(jì)，再構(gòu)造另一套濾波鏈過程——Process Two。

3．2．2 Process Two

在本節(jié)中，呈現(xiàn)另一套濾波過程，Process Two，該過程與Process One并行運(yùn)行，但對(duì)于System 1和System 2的調(diào)用與Process One相反，在Process Two中System 1先于System 2被調(diào)用。令Process Two得到的狀態(tài)在k時(shí)刻的估計(jì)為與也即是System 2處理完畢后得到的估計(jì);且設(shè)定由x0，P0經(jīng)過System 2更新完畢后得到的估計(jì)與作為Process Two的初始值。Process Two的算法過程與 Process One相仿，只是先對(duì)System 1進(jìn)行處理，得到的估計(jì)再參與到System 2的處理中，處理完畢后得到Process Two的估計(jì)，即SPUKF估計(jì)。該算法由k時(shí)刻的估計(jì)經(jīng)過System 1處理過程過程(49)～(60)及再經(jīng)過System 2處理過程(37)～(48)后得到Process Two在k+2時(shí)刻的估計(jì)。由于設(shè)置的初始值是，Process Two得到的實(shí)際是狀態(tài)在奇數(shù)時(shí)刻的估計(jì)，從而就能通過結(jié)合濾波鏈Process One及Process Two得到狀態(tài)在各個(gè)時(shí)刻的估計(jì)。

4 模擬仿真

鐘擺運(yùn)動(dòng)是一個(gè)典型的帶非線性等式約束的動(dòng)態(tài)系統(tǒng)范例，文獻(xiàn)［9］、文獻(xiàn)［10］、文獻(xiàn)［22］等都將其作為實(shí)例進(jìn)行仿真以展現(xiàn)約束濾波算法的效果。同樣地，為了展現(xiàn)所提出的濾波方法的估計(jì)效果，本文也采用了該動(dòng)態(tài)模型并在Matlab 2013a軟件上進(jìn)行了實(shí)驗(yàn)仿真。

4．1 RMSE

選取M次蒙特卡羅(Monte Carlo)模擬仿真結(jié)果的均方根誤差(Root Mean Square Error，RMSE)作為比較各種濾波方法估計(jì)性能的參考指標(biāo)，為避免狀態(tài)向量各分量的量綱對(duì)RMSE的影響，所以只考慮狀態(tài)各個(gè)分量的RMSE結(jié)果，狀態(tài)的第j個(gè)分量的均方根誤差RMSEj定義為:

其中:j=1，2，…，nx，i=1，2，…，N，這里N表示以T為采樣間隔仿真時(shí)間段內(nèi)的時(shí)刻總數(shù)，x^jk|k，i表示第i次蒙特卡羅模擬中狀態(tài)的第j個(gè)分量在k時(shí)刻的狀態(tài)估計(jì)，而表示狀態(tài)分量的真實(shí)值。類似地，也可以構(gòu)建出狀態(tài)的第j個(gè)分量在k時(shí)刻的均方根誤差

它能用于評(píng)估該種算法的估計(jì)穩(wěn)定性。

為了驗(yàn)證得到的估計(jì)對(duì)于約束的滿足程度，定義的約束均方根誤差RMSEc為:

同時(shí)，為了能更清楚地給出約束估計(jì)算法相對(duì)于標(biāo)準(zhǔn)UKF算法對(duì)估計(jì)效果的提升效果，引入了誤差改善比，約束算法的誤差改善比計(jì)算為:

狀態(tài)各個(gè)分量的誤差改善比也可以由上面的式子進(jìn)行計(jì)算。

4．2 鐘擺運(yùn)動(dòng)仿真結(jié)果

定義鐘擺在k時(shí)刻的狀態(tài)向量為xk=［θkωk］T，其中θk表示該時(shí)刻鐘擺所處的角度，ωk表示該時(shí)刻鐘擺的角速度，則關(guān)于鐘擺運(yùn)動(dòng)狀態(tài)模型的過程方程及觀測(cè)方程為:

其中:g是重力加速度，L指鐘擺的長(zhǎng)度，T表示離散化的時(shí)間步長(zhǎng)，并假定過程噪聲wk及觀測(cè)噪聲vk服從均值為0，協(xié)差 陣 分 別 為 Qk= diag(［0．0072，0．0072］) 和 Rk=diag(［0．12，0．12］) 的高斯分布。由機(jī)械能守恒定律知，該鐘擺系統(tǒng)應(yīng)滿足約束方程:

其中:C是機(jī)械能能量常數(shù)，其他變量的值設(shè)定為L(zhǎng)=1 m，T=0．05 s，鐘擺質(zhì)量m=1 kg，g=9．81 m/s2。給定系統(tǒng)狀態(tài)的初始值為x0=［π/4，π/50］T與P0=diag(［1，1］)，利用約束方程(65)可以計(jì)算出機(jī)械能常數(shù)C。仿真模擬數(shù)值結(jié)果見表 1、表 2。

表1 幾種約束UKF方法均方根誤差結(jié)果Tab．1 RMSE results of several constrained UKFs

表1給出了不帶約束無跡卡爾曼濾波(UKF)和并行無跡濾波(SPUKF)，分別基于擴(kuò)展卡爾曼濾波及無跡卡爾曼濾波兩種形式的批處理EKF估計(jì)器(Batch Processing Extended Kalman Filter，BchEKF)與批處理 UKF估計(jì)器(Batch Unscented Kalman Filter，Batch)，以及基于無跡卡爾曼濾波構(gòu)造的兩種形式的序貫LMMSE估計(jì)器［15，23］(分別記作Form 1和Form 2，其中Form 1表示觀測(cè)先于約束的處理次序的序貫LMMSE估計(jì)器形式，F(xiàn)orm 2表示與之相對(duì)的另一種處理次序的序貫LMMSE估計(jì)器)等方法在100次蒙特卡羅模擬中的均方根誤差的結(jié)果。由表中給出的結(jié)果可以看到:SPUKF與Batch的估計(jì)效果相當(dāng)，且明顯好于兩種基于無跡卡爾曼濾波的序貫估計(jì)器Form1、Form 2以及BchEKF，且SPUKF的估計(jì)效果也好于標(biāo)準(zhǔn)UKF估計(jì)，表明SPUKF這種約束算法有助于提升估計(jì)性能;SPUKF與批次形式及序貫形式的無跡卡爾曼濾波估計(jì)器的比較結(jié)果，也說明了不考慮觀測(cè)與約束處理次序的SPUKF方法的有效性。由約束均方根誤差的結(jié)果看到，由SPUKF得到的估計(jì)約束誤差較小，相對(duì)于 BchEKF、UKF、Form 1及Form 2狀態(tài)估計(jì)更滿足約束條件。此外，批次形式下的LMMSE估計(jì)方法與序貫形式下的LMMSE估計(jì)方法的巨大差異，也印證了前文指出的非線性約束情形下兩種形式的偽觀測(cè)法等價(jià)性失效的結(jié)論［15］。

表2給出了以上各種估計(jì)方法在使用Matlab仿真時(shí)的算法運(yùn)行時(shí)間情況以及 SPUKF、BchEKF、Batch、Form 1、Form 2等約束算法相對(duì)于標(biāo)準(zhǔn)UKF估計(jì)的均方根誤差改善百分比。由表中數(shù)據(jù)可以看到SPUKF的運(yùn)行時(shí)間與兩種序貫形式下的UKF算法Form 1、Form 2的運(yùn)行時(shí)間相差不大;由于中間進(jìn)行無跡變換的次數(shù)多于UKF，所以SPUKF的運(yùn)算時(shí)間要長(zhǎng)些;從位置的估計(jì)誤差改善比來看，SPUKF估計(jì)算法相對(duì)于標(biāo)準(zhǔn)UKF得到的估計(jì)，估計(jì)精度將提升22%左右，略低于Batch算法的33%;表中BchEKF，F(xiàn)orm 1及Form 2的誤差改善比為負(fù)數(shù)，表明這些算法在鐘擺運(yùn)動(dòng)的實(shí)例中估計(jì)誤差很大，遠(yuǎn)高于SPUKF、Batch。

表2 幾種約束UKF方法運(yùn)行時(shí)間及估計(jì)效果Tab．2 Running time and estimate quality of several constrained UKFs

圖2及圖3分別給出了100次蒙特卡羅仿真結(jié)果中各類約束UKF方法在各個(gè)時(shí)刻的鐘擺位置及角速度的均方根誤差結(jié)果。

由于BchEKF、Form 1及Form 2這三種估計(jì)方法的估計(jì)效果比較不佳，故在圖像中不予呈現(xiàn)。從圖中均方根誤差曲線可以看到，在穩(wěn)定階段時(shí)，SPUKF估計(jì)方法的誤差曲線基本與批處理無跡卡爾曼濾波方法Batch曲線相重合，且低于不帶約束的UKF誤差曲線，表明SPUKF的效果與批處理形式的無跡卡爾曼濾波算法估計(jì)效果相當(dāng)，也再次表明了文中所提的UKF方法的有效性。

圖2 各約束UKF算法鐘擺位置均方誤差結(jié)果Fig．2 RMSE results of pendulum position in constrained UKFs

5 結(jié)語(yǔ)

本文中提出了一種新形式的基于無跡卡爾曼濾波的偽觀測(cè)法估計(jì)器，用以處理帶非線性等式約束情形下的狀態(tài)估計(jì)問題，且在序貫處理觀測(cè)及約束時(shí)，無需確定二者的處理次序，故能解決序貫處理算法中關(guān)于觀測(cè)及約束處理的次序優(yōu)先性的問題，而且該種方法能獲得與批處理方法相當(dāng)?shù)墓烙?jì)效果，并通過對(duì)鐘擺運(yùn)動(dòng)的蒙特卡羅仿真模擬予以證實(shí)。構(gòu)造分離并行子系統(tǒng)以及雙濾波鏈形式，是關(guān)于帶等式約束估計(jì)問題的有益嘗試。關(guān)于SPUKF在其他方面的應(yīng)用，如線性約束條件、狀態(tài)模型不精確情形等，該種方法與其他約束濾波算法估計(jì)性能比較，以及將雙濾波鏈形式交叉融合這種形式應(yīng)用到其他濾波算法情形從而構(gòu)造關(guān)于帶約束狀態(tài)的估計(jì)方法還需要后續(xù)不斷的研究。

圖3 各約束UKF算法鐘擺角速度均方誤差結(jié)果Fig．3 RMSE results of pendulum velocity in constrained UKFs