李麗錦，龐驥庭，周志廣

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

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

李麗錦，龐驥庭，周志廣

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

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

非線性；卡爾曼濾波；發(fā)散；自適應濾波

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

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

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

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

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

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

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

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

定義：

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

則線性化系統(tǒng)化為：

時變噪聲統(tǒng)計估值器為：

從初值出發(fā)，先用式（5）計算狀態(tài)估值，然后用式（6）計算下一步噪聲統(tǒng)計估值，重復上述兩段，構(gòu)成狀態(tài)噪聲統(tǒng)計估值的兩段魯棒卡爾曼濾波算法。

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

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

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

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

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

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

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

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

4　仿真結(jié)果和評估

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

其中：

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

Z為速度誤差量測量。

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

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

參數(shù)設置如下：設當?shù)氐乩砭暥葹椋?4.2°，水平失準角為1°，方位失準角為10°，先驗噪聲是實際噪聲方差的50倍。由于北向水平失準角的估計結(jié)果與東向失準角的估計結(jié)果類似，僅將東向水平失準角的估計誤差為例，非線性強跟蹤自適應濾波下方位失準角的估計誤差和東向失準角的估計誤差如圖2、圖3所示。

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

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

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

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

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

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

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

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

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

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

5　結(jié)　論

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

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

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

［3］侯俊，朱長青.基于隨機加權(quán)估計的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—），女，河南平頂山人，碩士研究生，工程師。研究方向：無人機機載測量設備技術(shù)。