湯 啟，何臘梅

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

2 無跡卡爾曼濾波

2．1 無跡變換

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

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

對得到的各個 sigma點應用非線性變換 g(·)，得到γk，i=g(χk，i)(i=0，1，2，…，2nx)，而關于隨機變量 zk的估計結果即是:

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

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

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

觀測更新階段的過程步驟有

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

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

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

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

2．3 約束處理方式

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

2．3．1 偽觀測法

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

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

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

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

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

2．3．2 約束同化法

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

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

k，Pdkdk及Pxkdk由下式得到:

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

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

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

3 并行無跡濾波

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

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

3．2 SPUKF 算法過程

SPUKF算法由兩套濾波更新過程 (即濾波鏈)所組成，分別記作成Process One和 Process Two。這兩條濾波過程是由System 1及System 2不同的先后調(diào)用次序而得到，即兩條濾波過程各自對應于序貫LMMSE估計器的兩套形式，F(xiàn)orm 1與Form 2，而每套濾波過程得到的是間隔兩個采樣時間間隙2T的時刻的狀態(tài)估計。Process One和Process Two所獲得的估計相組合即得到SPUKF方法的估計結果，顯然地，只考慮其中某一套過程鏈不能得到狀態(tài)在各個時刻的估計結果，所以需要將該兩種濾波鏈進行齒合才能得到完整的各個時刻的估計，就像用拉鏈將兩條鏈帶銜接起來一樣，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得到的更新過后的結果將會作為下一時刻的先驗估計向System 1傳遞，之后接著將得到的估計返送回System 2，設定System 1更新完成后的結果作為Process One的估計，并設定 Process One 的初始值為 x0、P0。令 k=1，2，3，…，Process One的具體算法步驟如下:

對于 k=1，2，…

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

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

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

從上面的計算過程可以看到，在Process One濾波鏈中，由k－1時刻的估計推更新得到了 k+1時刻的估計，又因為初始值設置為x0與P0，這說明利用Process One濾波鏈能得到狀態(tài)向量在偶數(shù)時刻的估計。為了補齊狀態(tài)在奇數(shù)時刻的估計，再構造另一套濾波鏈過程——Process Two。

3．2．2 Process Two

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

4 模擬仿真

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

4．1 RMSE

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

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

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

為了驗證得到的估計對于約束的滿足程度，定義的約束均方根誤差RMSEc為:

同時，為了能更清楚地給出約束估計算法相對于標準UKF算法對估計效果的提升效果，引入了誤差改善比，約束算法的誤差改善比計算為:

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

4．2 鐘擺運動仿真結果

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

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

其中:C是機械能能量常數(shù)，其他變量的值設定為L=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)可以計算出機械能常數(shù)C。仿真模擬數(shù)值結果見表 1、表 2。

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

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

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

表2 幾種約束UKF方法運行時間及估計效果Tab．2 Running time and estimate quality of several constrained UKFs

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

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

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

5 結語

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

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