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

李麗錦，龐驥庭，周志廣

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

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

李麗錦，龐驥庭，周志廣

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

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

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

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

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

1　非線性強(qiáng)跟蹤自適應(yīng)濾波器

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

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

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

2　非線性Sage-Husa自適應(yīng)濾波

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

定義：

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

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

時(shí)變?cè)肼暯y(tǒng)計(jì)估值器為：

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

3　改進(jìn)型非線性自適應(yīng)濾波

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

圖1　改進(jìn)型非線性自適應(yīng)濾波算法流程圖

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

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

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

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

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

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

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

其中：

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

Z為速度誤差量測(cè)量。

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非線性強(qiáng)跟蹤自適應(yīng)濾波

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

圖2　方位失準(zhǔn)角的估計(jì)誤差

圖3　東向失準(zhǔn)角的估計(jì)誤差

4.2非線性Sage-Husa自適應(yīng)濾波

非線性Sage-Husa自適應(yīng)濾波下方位失準(zhǔn)角的估計(jì)誤差和東向失準(zhǔn)角的估計(jì)誤差如圖4、圖5所示。

4.3改進(jìn)型非線性自適應(yīng)濾波

改進(jìn)型非線性自適應(yīng)濾波下方位失準(zhǔn)角的估計(jì)誤差和東向失準(zhǔn)角的估計(jì)誤差如圖6、圖7所示。

圖4　方位失準(zhǔn)角的估計(jì)誤差

圖5　東向失準(zhǔn)角的估計(jì)誤差

圖6　方位失準(zhǔn)角的估計(jì)誤差

圖7　東向失準(zhǔn)角的估計(jì)誤差

5　結(jié)　論

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

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

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

［3］侯俊，朱長(zhǎng)青.基于隨機(jī)加權(quán)估計(jì)的Sage自適應(yīng)濾波及其在導(dǎo)航中的應(yīng)用［J］.電子設(shè)計(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］沈鶴鳴，華承湘.一種復(fù)合型自適應(yīng)Kalman濾波器的仿真研究［J］.控制與決策，1990，5（3）：51-56.

［6］夏天維，侯翔.基于自適應(yīng)Kalman濾波的機(jī)器人運(yùn)動(dòng)目標(biāo)跟蹤算法［J］.計(jì)算機(jī)測(cè)量與控制，2015，23（1）：173-175.

［7］時(shí)也，吳懷宇.基于擴(kuò)展卡爾曼濾波器的移動(dòng)機(jī)器人SLAM研究［J］.電子設(shè)計(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稿件編號(hào)：201504272

李麗錦（1982—），女，河南平頂山人，碩士研究生，工程師。研究方向：無(wú)人機(jī)機(jī)載測(cè)量設(shè)備技術(shù)。