基于內(nèi)點法的不敏卡爾曼濾波算法

賀姍+師昕

摘要：針對非線性不等式狀態(tài)約束濾波問題，提出一種基于內(nèi)點法的不敏卡爾曼濾波算法。該算法在不敏卡爾曼濾波的基礎(chǔ)上結(jié)合了優(yōu)化算法的思想，采用內(nèi)點優(yōu)化法求解非線性不等式約束條件下的最優(yōu)解。在算法實現(xiàn)過程中，引入障礙項，用無約束障礙函數(shù)近似化受約束目標(biāo)函數(shù)，采用一個相當(dāng)小的正數(shù)即障礙因子，通過序列極小化方法逐漸減小該障礙因子，經(jīng)過迭代快速搜索出非線性不等式狀態(tài)約束問題的近似最優(yōu)解。對具有約束的航路跟蹤系統(tǒng)進行實驗仿真，實驗結(jié)果表明新算法在處理非線性不等式狀態(tài)約束濾波問題時，能夠有效地提高狀態(tài)估計精度，獲得較高的濾波精度，且時間復(fù)雜度較低。

關(guān)鍵詞：狀態(tài)約束；不敏卡爾曼濾波；優(yōu)化算法；目標(biāo)跟蹤；信息融合

DOIDOI：10.11907/rjdk.171335

中圖分類號：TP312

文獻標(biāo)識碼：A 文章編號：1672-7800（2017）006-0040-05

0 引言

在狀態(tài)估計問題中，狀態(tài)向量可能受到很多因素制約。例如：船只在海上航行中會受到航路的約束，鐘擺擺動過程中需要遵守機械能守恒定律。若將這些約束條件有效地施加到狀態(tài)估計過程中，能夠更加逼近實際狀態(tài)值，獲得較高的濾波精度[1-3]。因此，針對受約束條件下狀態(tài)估計問題的研究具有重要意義。

近年來，人們針對等式狀態(tài)約束問題進行了深入研究，并提出了很多有效的約束算法。例如：水平滑動估計算法（Moving Horizon Estimation， MHE）[4-5]、平滑約束卡爾曼濾波（Smoothly Constrained Kalman Filter， SCKF）[6]、二階泰勒級數(shù)展開非線性約束濾波（2ord Nonlinear Constrained，2ord NC）[7]等。在實際問題中，系統(tǒng)狀態(tài)向量也可能受到不等式約束條件的限制。然而，針對該問題的研究成果較少。2002年，Simon針對不等式約束問題提出了概率密度截斷算法[8]，該算法將不等式約束函數(shù)轉(zhuǎn)換為概率密度函數(shù)，能夠獲得較高的濾波精度，但是其計算量會隨著狀態(tài)維數(shù)的增大而迅速增大，因而不適合狀態(tài)維數(shù)較高的約束問題。Vincent等[9]提出了一種模糊逼近的方法，即將不等式約束條件視為帶有模糊噪聲的量測值，然后利用拉格朗日乘子法迭代求解出受約束后下的狀態(tài)估計值，然而其受限于隨時間變化的約束問題。2010年，Simon[3]提出了積極集法，該方法利用先驗等式約束方程對不滿足不等式約束的估計值進行逼近，取得了較好效果。本文結(jié)合優(yōu)化算法的思想，采用內(nèi)點法[10]解決不等式狀態(tài)約束濾波問題。該算法引入障礙項，將約束問題轉(zhuǎn)換成無約束問題，即用障礙函數(shù)代替原目標(biāo)函數(shù)，利用一個相當(dāng)小的正數(shù)即障礙因子，采用序列極小化的方法，逐漸減小障礙因子，經(jīng)過迭代，獲得不等式約束濾波問題的近似最優(yōu)解[11-13]。該算法為非線性不等式約束濾波問題提供了一種新的實現(xiàn)途徑，并具有較好的濾波結(jié)果。

3 非線性不等式約束濾波最優(yōu)解求解

經(jīng)過上述UKF算法濾波后，得到濾波更新值k|k和Pk|k，此時需要通過非線性不等式約束條件對k|k進行修正，以達到約束優(yōu)化的目的。為解決該非線性不等式約束問題，本文采用內(nèi)點法。該算法在優(yōu)化過程中引入了障礙項，用獲得的障礙函數(shù)代替原目標(biāo)函數(shù)，即可將復(fù)雜的約束問題轉(zhuǎn)換成無約束問題。然后利用一個相當(dāng)小的正數(shù)，即障礙因子，采用序列極小化的方法，逐漸減小障礙因子，迭代求解出非線性不等式約束濾波最優(yōu)解。內(nèi)點法在可行域周圍筑起一道“障礙”，一旦搜索點接近該邊界時，通過迅速增大障礙函數(shù)，迫使迭代點始終都在可行域內(nèi)部。當(dāng)搜索點遠(yuǎn)離可行域邊界時，則用障礙函數(shù)近似目標(biāo)函數(shù)，此時，可以將障礙函數(shù)的無約束最優(yōu)解作為帶約束目標(biāo)函數(shù)的最優(yōu)解。針對式（5）所考慮的優(yōu)化問題，其可行域為：

由式（41）可知該系統(tǒng)為非線性，濾波過程采用不敏卡爾曼濾波算法。在此基礎(chǔ)上，通過約束式對濾波結(jié)果進行修正，以提高濾波精度。本文分別從位置均方根誤差和速度均方根誤差兩個方面對比各算法的誤差性能，實驗結(jié)果如圖1和圖2所示。由圖1可知，分別采用無約束UKF算法和IP-UKF算法對目標(biāo)進行濾波后，IP-UKF算法的位置均方根誤差小于無約束UKF算法的位置均方根誤差，圖2給出了兩種濾波算法速度均方根誤差的對比結(jié)果，從圖中易知，經(jīng)IP-UKF算法濾波后的速度均方根誤差小于無約束的UKF算法。從上述兩方面分析結(jié)果可知IP-UKF算法與一般的無約束UKF算法相比能夠取得更高的濾波精度，濾波結(jié)果更好地收斂于真實值。盡管在個別時刻出現(xiàn)了波峰值，但是IP-UKF算法的整體性能和無約束UKF算法相比表現(xiàn)較好。圖3和圖4為無約束UKF算法和AS-UKF算法濾波之后的均方根誤差對比結(jié)果，由圖可知，經(jīng)過AS-UKF算法濾波后的位置和速度均方根誤差均小于無約束的UKF算法的均方根誤差。

為了更加清晰比對出本文提出的IP-UKF算法和AS-UKF算法的性能優(yōu)越性，采用100次蒙特卡羅仿真實驗，其結(jié)果如表1所示。由表1可知，經(jīng)過100次蒙特卡羅實驗之后，IP-UKF算法與AS-UKF算法相比較，位置和速度均方根誤差相當(dāng)，而時間復(fù)雜度稍低，且IP-UKF算法實施過程根本不同于AS-UKF算法：AS-UKF算法不能直接應(yīng)用于不等式狀態(tài)約束估計，具體實現(xiàn)過程中首先需要判斷濾波結(jié)果是否符合約束條件，如果滿足，則不進行約束處理；否則，采用先驗等式約束條件對濾波結(jié)果進行約束限制，最終得到約束后的濾波結(jié)果。且AS-UKF算法在實施過程中會經(jīng)常遇到困難，其計算量會隨著約束條件數(shù)的增加而呈指數(shù)增長。綜上所述，本文提出的IP-UKF算法能夠有效地利用約束條件對狀態(tài)值進行修正，算法時間復(fù)雜度較低，為解決解決具有非線性不等式狀態(tài)約束的濾波問題提出了一種新的實現(xiàn)途徑。

5 結(jié)語

針對非線性不等式狀態(tài)約束問題，本文提出了一種新的算法，即基于內(nèi)點法的不敏卡爾曼濾波算法。實施過程中引入了障礙項，其特點是能夠?qū)⑤^復(fù)雜的受約束濾波問題轉(zhuǎn)化為無約束濾波問題，從而可利用最優(yōu)化理論解決該問題。其具體實現(xiàn)是在可行域內(nèi)通過迭代方法，促使障礙函數(shù)和目標(biāo)函數(shù)盡可能地接近，障礙函數(shù)求得的最優(yōu)解即可作為約束濾波問題的最優(yōu)解。本文算法為解決該類非線性不等式狀態(tài)約束濾波問題提供了一種新的實現(xiàn)途徑，通過對系統(tǒng)狀態(tài)向量進行約束限制，可修正狀態(tài)估計值，提高狀態(tài)估計精度。仿真實驗結(jié)果表明，本文提出的IP-UKF算法具有較小的誤差值，可以更加精確地估計出狀態(tài)值，使?fàn)顟B(tài)估計值更好地收斂于真實狀態(tài)值，有效地提高了濾波精度，是一種可行的非線性不等式約束濾波算法。

參考文獻：

[1]陳金廣， 李潔， 高新波. 一種迭代收縮非線性狀態(tài)約束濾波算法[J]. 西安電子科技大學(xué)學(xué)報，2011， 38（1）： 104-109.

[2]JULIER S，UHLMANN J， DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transactions on Automatic Control， 2000， 45（3）： 477-482.

[3]SIMON D. Kalman filtering with state constraints： a survey of linear and nonlinear algorithms[J]. IET Control Theory & Applications， 2010， 4（8）： 1303-1318.

[4]MICHALSKA H， MAYNE D. Moving horizon observers and observer based control [J]. IEEE Transactions on Automatic Control， 1995， 40（6）： 995-1006.

[5]RAO C， RAWLIAGS J， LEE J. Constrained linear stateestimation—a moving horizon approach[J]. Automatica，2001， 37（10）： 1619-1628.

[6]DE GEETER J， VAN BRUSSEL H， DE SCHUTTER J. A smoothly constrained Kalman filter[J].IEEE Transactions on Pattern Analysis and Machine Intelligence， 1997， 19（10）： 1171-1177.

[7]YANG C， BLASCH E. Kalman filtering with nonlinear state constraints[J]. IEEE Transactions on Aerospace and Electronic Systems， 2009， 45（1）： 70-83.

[8]SIMON D， CHIA T. Kalman filtering with state equality constraints[J].IEEE Transactions on Aerospace and Electronic Systems， 2002， 38（1）： 128-136.

[9]VINCENT S， GHALEB H， HOUCINE C. State estimation under nonlinear state inequality constraints：a tracking application[C].16th Mediterranean Conference on Control and Automation. France： IEEE，2008：1669-1674.

[10]BRADLEY M B， JAMES V B， GIANLUIGI P. An inequality constrained nonlinear Kalman-Bucy smoother by interior pointlikelihood maximization[J].IEEE Transactions on Automatic Control，2009.

[11]BOONANS J， GILBERT J. Numerical optimization：theoretical and practical aspects[M].New York： Spring， 2000：51-65，353-368.

[12]JIANG Q， GENG G C. A reduced-space interior point method for transient stability constrained optimal power flow[J]. IEEE Transactions on Power Systems， 2010，25（3）： 1232-1240.

[13]SIMON D.Optimal state estimation[M].New Jersey：John Wiley & Sons， 2006： 218-222.

[14]JULIER S，UHLMANN J， DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transactions on Automatic Control，2000，45（3）：477-482.

（責(zé)任編輯：陳福時）