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

賀姍+師昕

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

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

DOIDOI：10.11907/rjdk.171335

中圖分類號(hào)：TP312

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

0 引言

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

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

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

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

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

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

5 結(jié)語

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

參考文獻(xiàn)：

[1]陳金廣， 李潔， 高新波. 一種迭代收縮非線性狀態(tài)約束濾波算法[J]. 西安電子科技大學(xué)學(xué)報(bào)，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é)任編輯：陳福時(shí)）