基于內點法的不敏卡爾曼濾波算法

賀姍+師昕

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

關鍵詞：狀態約束；不敏卡爾曼濾波；優化算法；目標跟蹤；信息融合

DOIDOI：10.11907/rjdk.171335

中圖分類號：TP312

文獻標識碼：A 文章編號：1672-7800（2017）006-0040-05

0 引言

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

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

3 非線性不等式約束濾波最優解求解

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

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

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

5 結語

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

參考文獻：

[1]陳金廣， 李潔， 高新波. 一種迭代收縮非線性狀態約束濾波算法[J]. 西安電子科技大學學報，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.

（責任編輯：陳福時）