針對非線性卡爾曼濾波算法發散的抑制方法研究

李麗錦，龐驥庭，周志廣

（西北工業大學 第365研究所，陜西 西安　710065）

針對非線性卡爾曼濾波算法發散的抑制方法研究

李麗錦，龐驥庭，周志廣

（西北工業大學 第365研究所，陜西 西安710065）

針對常用的非線性擴展卡爾曼濾波算法在工程應用中所存在的發散問題，文中分析歸納了導致該算法發散的主要原因，同時在目前兩種抑制濾波發散非線性算法的基礎上，探討了一種既保證濾波精度又提高自適應能力的改進型自適應濾波算法。并通過捷聯慣導系統的初始對準誤差模型對3種濾波算法進行數學仿真。仿真結果表明，改進型非線性自適應濾波可有效地抑制濾波發散，并在提高誤差估計精度的基礎上具有較大范圍的自適應能力。

非線性；卡爾曼濾波；發散；自適應濾波

擴展卡爾曼濾波以將非線性濾波 “線性化”的方法廣泛應用于非線性系統中如：飛機和艦船上的慣導系統、導彈的制導系統以及其他很多工業控制系統等［1］。但在應用卡爾曼濾波器有時會出現這樣一種現象：當量測值數目不斷增大時，按濾波方程計算的估計均方誤差趨于零或趨于某一穩態值，估計值相對實際的被估計值的偏差卻越來越大，使濾波器逐漸失去估計作用，這種現象稱為濾波器的發散?？梢宰C明：即使理論上證明卡爾曼是漸進穩定的，并不能保證濾波器算法在實際上具有收斂性。

文中針對擴展卡爾曼濾波的工程應用中的發散問題，在非線性強跟蹤自適應濾波器和非線性Sage-Husa自適應濾波的基礎上，探討了一種既保證濾波精度又提高自適應能力的改進型自適應濾波算法。

1　非線性強跟蹤自適應濾波器

當系統模型不準確時，新量測值對估計值的修正作用下降，陳舊量測值的修正作用相對上升是引發濾波發散的一個重要原因。因此逐增加衰減因子漸減小陳舊量測值的權重，相應的增大新量測值的權重，這是扼制濾波發散的一個可行途徑，強跟蹤自適應濾波是通過這種途徑扼制濾波發散的一種次優濾波方法。

結合擴展卡爾曼濾波算法，得到如下非線性強跟蹤自適應濾波算法［2］：

該算法對濾波器估計誤差驗前協方差矩陣加權處理，增強量測信息的修正作用可以抑制濾波器的發散。

2　非線性Sage-Husa自適應濾波

最優卡爾曼濾波器要求精確已知系統的模型 （包括噪聲統計），然而在許多實際應用問題中模型常常是近似的、帶有未知誤差的。這種未知的模型誤差叫做未建模動態，它是相對于假設存在系統的一個精確的模型而言的。增加虛擬噪聲和分別補償了線性化狀態模型和線性化量測方程的誤差，把泰勒展開式高階項合并到虛擬噪聲中去。因為該線性化系統含有未建模動態，它們被歸入虛擬噪聲中去加以補償。這種自適應卡爾曼濾波器叫魯棒卡爾曼濾波器。

定義：

帶未知或已知噪聲統計的非線性Sage-Husa自適應濾波算法如下［3］：

則線性化系統化為：

時變噪聲統計估值器為：

從初值出發，先用式（5）計算狀態估值，然后用式（6）計算下一步噪聲統計估值，重復上述兩段，構成狀態噪聲統計估值的兩段魯棒卡爾曼濾波算法。

3　改進型非線性自適應濾波

非線性Sage-Husa自適應濾波與非線性強跟蹤卡爾曼濾波各有優缺點，前者精度較高但計算量大，實時性和穩定性難以保證，后者結構簡單，可靠性高、自適應能力強，但是它的濾波精度降低了。如果將它們結合起來，就能充分發揮二者優勢，構成既可抑制濾波發散又能保證濾波精度和效率的改進自適應濾波算法。這種方法首先利用收斂性判據判斷濾波器的發散趨勢，若不發散，采用非線性Sage-Husa自適應濾波算法，存在發散趨勢就采用非線性強跟蹤自適應濾波。其算法流程圖如圖1所示。

圖1　改進型非線性自適應濾波算法流程圖

為了防止算法發散，必須首先選擇可以判斷一般卡爾曼濾波器是否發散的判據。當濾波器發散時，誤差協方差陣無界，這時實際的估計誤差往往比理論預計的誤差大很多倍，根據這一特點，采用下列不等式作為判據［5］：

式中γ為儲備系數 （γ＞1）；tr為矩陣跡的符號；ek為更新序列：

與協方差矩陣的跡有關，它反映了理論預計誤差的信息。這里協方差矩陣的理論值為：

用矩陣的理論值去代替E［ekeTk］，得到判據的最終形式如下：

當式（10）成立時，而當上式（10）不成立時，表明濾波器的實際估計誤差將超過理論預計值γ倍以上，此時濾波器處于發散狀態。

4　仿真結果和評估

以捷聯慣導系統初始對準誤差估計為例，不考慮垂直通道，考慮系統噪聲，把誤差擴充為狀態向量，最后得到方位角為大失準角下的初始對準誤差方程為：

其中：

w=［wgxwgywgzwaxway］是陀螺和加速度計的白噪聲。

Z為速度誤差量測量。

G為10×5維的矩陣系數，G（1，1）=1，G（2，2）=1，G（3，3）=1，G（4，4）=1，G（5，5）=1。

4.1非線性強跟蹤自適應濾波

參數設置如下：設當地地理緯度為：34.2°，水平失準角為1°，方位失準角為10°，先驗噪聲是實際噪聲方差的50倍。由于北向水平失準角的估計結果與東向失準角的估計結果類似，僅將東向水平失準角的估計誤差為例，非線性強跟蹤自適應濾波下方位失準角的估計誤差和東向失準角的估計誤差如圖2、圖3所示。

圖2　方位失準角的估計誤差

圖3　東向失準角的估計誤差

4.2非線性Sage-Husa自適應濾波

非線性Sage-Husa自適應濾波下方位失準角的估計誤差和東向失準角的估計誤差如圖4、圖5所示。

4.3改進型非線性自適應濾波

改進型非線性自適應濾波下方位失準角的估計誤差和東向失準角的估計誤差如圖6、圖7所示。

圖4　方位失準角的估計誤差

圖5　東向失準角的估計誤差

圖6　方位失準角的估計誤差

圖7　東向失準角的估計誤差

5　結　論

對比的仿真結果研究表明：非線性強跟蹤自適應卡爾曼濾波算法算法結構簡單，在其克服濾波器發散的過程中，破壞了濾波器的最優條件，是以損失濾波精度為代價來換取濾波的收斂，收斂速度較慢；非線性Sage-Husa自適應濾波，收斂速度快，但濾波值大起大落，若b值選取不合適容易出現發散現象。而改進型自適應卡爾曼濾波算法在模型誤差和較大的初值誤差情況下，仍不會出現發散現象。在此，改進型非線性自適應濾波算法能在線估計噪聲統計特性起到了關鍵作用，當然，此時濾波值有些波動，這是由于濾波器處于次優濾波狀態。在一定模型誤差場合，通過選取合適的b值后，整個改進型非線性自適濾波算法的濾波精度優于非線性強跟蹤自適應卡爾曼濾波算法，對先驗噪聲統計特性和系統數學模型依賴小，自適應能力強。

［1］高孟茹，陳光.基于自適應卡爾曼濾波的新船舶試航系統［J］.電子設計工程，2014，3（6）：46-49.

［2］秦永元，張洪鉞，等.卡爾曼濾波與組合導航原理［M］.西北工業出版社，2004.

［3］侯俊，朱長青.基于隨機加權估計的Sage自適應濾波及其在導航中的應用［J］.電子設計工程，2014，10（20）：1-5.

［4］Carlos A A，Rajesh E M，Zhou C J，et al.A model architecture for humanoid soccer robots with distributed behavior control ［J］.Int J of Humanoid Robotics，2012，5（3）：397.

［5］沈鶴鳴，華承湘.一種復合型自適應Kalman濾波器的仿真研究［J］.控制與決策，1990，5（3）：51-56.

［6］夏天維，侯翔.基于自適應Kalman濾波的機器人運動目標跟蹤算法［J］.計算機測量與控制，2015，23（1）：173-175.

［7］時也，吳懷宇.基于擴展卡爾曼濾波器的移動機器人SLAM研究［J］.電子設計工程，2012，1（1）：104-106.

Research on methods of restrainning algorithm divergence for nonlinear Kalman filter

LI Li-jin，PANG Ji-ting，ZHOU Zhi-guang
（No.365 Research Institute，Northwestern Polytechnical University，Xi'an 710065，China）

For nonlinear extend Kalman filter algorithm divergence existed in the practice，this paper analyses the main reasons leading to filter divergence.Based on two nonlinear algorithms for restrainning algorithm divergence，a improved nonlinear adaptive fiter is adapted.By SINS（Strap-down Inertia Navigation System）initial alignment error model，these three filters are used to simulate the nonlinear system.Simulation results show that the improved nonlinear adaptive filter can effectively restrain the divergence，improve the accuracy of error estimation and have a large range of adaptive capacity.

nonlinear；Kalman filter；divergence；adaptive fiter

TN96

A

1674－6236（2016）06-0165-03

2015-04-24稿件編號：201504272

李麗錦（1982—），女，河南平頂山人，碩士研究生，工程師。研究方向：無人機機載測量設備技術。