基于IGGIII 方案的自適應(yīng)漸消卡爾曼濾波器

賀軍義，楊 豐，安葳鵬，尚家澤

河南理工大學 計算機科學與技術(shù)學院，河南 焦作 454000

1 引言

自適應(yīng)漸消卡爾曼濾波器（Adaptive Fading Kalman Filter，AFKF）通過引入漸消因子擴大準確量測信息的權(quán)重，能在一定程度上抑制模型建立不準確或模型參數(shù)突變時標準卡爾曼濾波器（Kalman Filter，KF）容易發(fā)散的問題，使濾波精度以及動態(tài)性能方面都有了相應(yīng)的改善[1-3]，但是該方法使用固定長度的滑動采樣區(qū)間，計算出的漸消因子在不同噪聲環(huán)境下，濾波調(diào)節(jié)能力具有單一性，不利于濾波精度的提高。文獻[4]提出一種計算多重漸消因子的方法，并根據(jù)均方誤差把漸消因子分配給各個濾波通道，提高了自適應(yīng)卡爾曼濾波算法的濾波精度，多重漸消因子的引入使計算量倍增，存在空間復(fù)雜度上升和實時性下降的問題。文獻[5]對不同滑動采樣區(qū)間長度采用加權(quán)組合思想求取新息協(xié)方差估計值，再采用交互多模算法進行信息融合，從而獲得精度更高的濾波估計值，但該算法加權(quán)計算過程使時間復(fù)雜度明顯增大，濾波效率嚴重下降。文獻[6]通過構(gòu)建噪聲統(tǒng)計特性檢測函數(shù)進行量測噪聲方差陣的梯度檢測，然后根據(jù)檢測結(jié)果利用窗口自適應(yīng)函數(shù)實時計算窗口寬度，避免滑動窗口長度固定帶來的濾波精度與跟蹤靈敏度之間的矛盾問題，然而該算法梯度檢測函數(shù)對3組預(yù)設(shè)量測噪聲方差修正系數(shù)進行求和運算，再進行加和運算求取最終梯度檢測值，影響了算法濾波實時性。文獻[7]對開窗估計法求取的協(xié)方差陣估計值和通過新息序列推求的協(xié)方差陣分別求跡，再將兩個跡的值相除直接得到漸消因子，簡化了漸消因子的計算過程，使運算效率有所提高，但該算法計算漸消因子時忽略了較小的觀測噪聲矩陣，造成濾波精度下降。

為解決上述現(xiàn)有算法所存在的問題，本文針對自適應(yīng)漸消卡爾曼濾波器滑動采樣區(qū)間長度選取進行了細致研究，提出了一種基于IGG（Institute of Geodesy and Geophysics，Chinese Academy of Sciences）III方案的自適應(yīng)漸消卡爾曼濾波器（IGGAFKF）。該濾波器首先利用新息馬氏距離服從卡方分布作為判斷準則進行新息異常檢測，然后根據(jù)不同檢測結(jié)果，通過構(gòu)建一個類似于IGGIII權(quán)函數(shù)的三段式滑動采樣區(qū)間長度N的選取表達式，求取不同噪聲環(huán)境下的滑動采樣區(qū)間長度。改進濾波算法的滑動采樣區(qū)間長度為在線求取，能夠?qū)崿F(xiàn)估計精度與動態(tài)性之間平衡的同時，兼顧濾波器實時性要求。Matlab仿真驗證了基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波算法的正確性和有效性。

2 自適應(yīng)漸消卡爾曼濾波器

標準卡爾曼濾波器在線性離散系統(tǒng)下的狀態(tài)方程和量測方程分別如下[8-10]：

式中，Xk為被估計的狀態(tài)變量，Zk為量測變量，Ak,k-1為狀態(tài)轉(zhuǎn)移矩陣，Bk為測量矩陣。Wk和Vk分別為系統(tǒng)噪聲和觀測噪聲，系統(tǒng)噪聲Wk已知且量測噪聲Vk為方差時變的零均值白噪聲，各個時刻噪聲未知且互不相關(guān)，其方差分別為

標準卡爾曼濾波器算法濾波過程為[13]：

式中，vk為新息，Dk為新息方差，Kk為卡爾曼增益矩陣。

文獻[14]提出的漸消卡爾曼濾波器將式（4）中引入漸消因子，即用式（8）代替式（4）：

則式（3）（8）（5）（6）（7）構(gòu)成了自適應(yīng)漸消卡爾曼濾波算法。漸消因子λk大于等于1，當計算出的漸消因子變?yōu)闃藴士柭鼮V波器[15]。實際值小于1時取1，即自適應(yīng)漸消卡爾曼濾波器：

式中，Dk為滑動取值長度為N時的新息方差估計值[16]：

與標準卡爾曼濾波器相比，漸消卡爾曼濾波器通過引入漸消因子使估計均方誤差增大為原來的λk倍，進而擴大增益矩陣Kk，增加了新近觀測信息所占權(quán)重，從而提升算法對準確數(shù)據(jù)的動態(tài)跟蹤性能，達到改善濾波精度的目的。

3 基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波器

自適應(yīng)漸消卡爾曼濾波算法中漸消因子求解方式存在顯著缺點，制約了濾波精度的提高。式（10）利用滑動采樣區(qū)間內(nèi)數(shù)據(jù)進行新息方差的估計時，滑動采樣區(qū)間長度N難以確定。若選擇的N值過大，開窗法求取的新息方差估計偏小，造成漸消因子變小，漸消卡爾曼濾波器估計值更接近于標準卡爾曼估計值，導致濾波器在模型參數(shù)發(fā)生變化時，對準確觀測量的動態(tài)跟蹤性能下降。若選擇的N值過小，在模型穩(wěn)定情況下使求取的漸消因子變大，增加了濾波器對觀測量的動態(tài)跟蹤性能，但降低了準確一步預(yù)測值在濾波結(jié)果中的權(quán)重，濾波器魯棒性變差，易導致濾波精度降低。所以，通過不斷測試設(shè)定的單一固定采樣區(qū)間長度N無法滿足濾波器在不同噪聲環(huán)境下的濾波需求。實現(xiàn)滑動采樣區(qū)間長度N的自適應(yīng)求取，使其適應(yīng)不同的噪聲環(huán)境，是提高漸消卡爾曼濾波器濾波精度的根本措施。

3.1 新息異?？ǚ綑z測

新息vk服從零均值高斯分布，且觀測量Zk也服從高斯分布，新息馬氏距離的平方應(yīng)該服從χ2分布[18]：

式中，Mk為馬氏距離。

為了檢驗當前新息vk是否與假設(shè)模型兼容，并判別新息vk的可信程度，根據(jù)假設(shè)檢驗理論，設(shè)置一個顯著性水平α：

在新息信息中包含模型信息和觀測信息，對新息異常進行檢測是采取自適應(yīng)策略提高濾波精度的前提。新息是一個服從零均值正態(tài)分布的白噪聲序列[17]，即：波器中觀測信息相對準確，那么新息異常由模型或噪聲參數(shù)不準確引起。因此，采用新息馬氏距離服從卡方分布能夠有效檢測新息是否出現(xiàn)異常，并分析出造成新息異常的原因。式中顯著水平α表示新息統(tǒng)計量與假設(shè)模型兼容的概率，其大小決定了新息統(tǒng)計量門限值χ2m,α的大小。在實際工程運用中，由于難以精確選取系統(tǒng)噪聲，α較為合適的取值范圍是0.01～0.15[17]，采用該方法得到進行新息異常劃分的具體顯著水平α數(shù)值可通過實驗測得，并不需要在線迭代計算，不會造成濾波效率的大幅下降。

3.2 IGGIII權(quán)函數(shù)

IGGIII方案是一種類似于相關(guān)最小二乘估計的抗差估計方案[20]。該方案將觀測數(shù)據(jù)按照質(zhì)量劃分成有效信息、可用信息以及有害信息。對應(yīng)權(quán)函數(shù)中的保權(quán)區(qū)、降權(quán)區(qū)、淘汰區(qū)。基本實現(xiàn)了對有效信息的充分利用、可疑信息的限制利用以及對有害信息的排除，是一種適合處理量測數(shù)據(jù)的處理方案。IGGIII的權(quán)函數(shù)為：

式中， ||ui為標準化殘差，k0和k1為調(diào)和系數(shù)。

3.3 滑動采樣區(qū)間長度優(yōu)化選取

利用以上新息卡方檢測方法，并引入IGGIII信息分類思想，通過先驗經(jīng)驗和實驗測試設(shè)置顯著水平α1、α2。并設(shè)原滑動區(qū)間取值長度為N，調(diào)節(jié)因子為κ，且令βk=，則采用IGGIII方案進行自適應(yīng)優(yōu)化后的取值長度N?為：

式中，α1決定滑動采樣區(qū)間長度取值取最長的幾率，決定濾波器的估計精度；α2決定滑動采樣區(qū)間長度取值最短的幾率，決定濾波估計的動態(tài)跟蹤性能。所以α1取值應(yīng)該盡量使其對應(yīng)的門限值偏大，以獲取更高的穩(wěn)定性，α2應(yīng)該使其對應(yīng)的門限值偏小，在發(fā)生模型參數(shù)變化時具有更好的動態(tài)性。經(jīng)過多次實驗，本文α1、α2分別設(shè)置為0.10和0.01。

當濾波出現(xiàn)發(fā)散時，通過滑動區(qū)間長度取最短，增加濾波估計值中準確觀測信息的權(quán)重，使濾波估計值向準確觀測值方向收斂。并且通過式（10）可知，滑動區(qū)間取最短會使開窗法求取協(xié)方差估計值時的求和運算量降至最低，加快協(xié)方差估計值求取速度，使濾波結(jié)果實現(xiàn)快速收斂，提高濾波精度。

3.4 基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波器算法

結(jié)合新息卡方異常檢測和滑動采樣區(qū)間長度優(yōu)化選取的內(nèi)容，本文提出的基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波器算法工作流程如下：

步驟1 初始化賦值，X0、P0、α1、α2、N 。

步驟2通過式（3）進行一步狀態(tài)預(yù)測。

步驟3求取自適應(yīng)漸消因子。

步驟3.1求取新息向量：

步驟3.2通過式（15）進行新息卡方檢測和求取融入IGGIII方案思想的滑動區(qū)間長度。

步驟3.3求取新息方差最優(yōu)估計值：

步驟3.4將 D?k代入式（9）求取優(yōu)化后的漸消因子 λ?k。

步驟4將λ?k代入式（8）計算加入漸消因子修正后的系統(tǒng)誤差協(xié)方差。

步驟5通過式（5）～（7）計算卡爾曼增益、狀態(tài)最優(yōu)估計以及系統(tǒng)誤差協(xié)方差，進行濾波更新。

步驟6如果算法未結(jié)束，k=k+1，進行步驟2。否則進行步驟7。

步驟7結(jié)束。

由算法工作流程和圖1可知，基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波器濾波算法在進行滑動采樣區(qū)間自適應(yīng)求取時將新息馬氏距離服從卡方分布作為判斷準則進行新息異常判斷，并引入IGGIII信息分類方案的思想，把新息分為可靠信息、可疑信息和有害信息三類，分析了造成新息出現(xiàn)波動和異常的原因，分別采取不同的滑動采樣區(qū)間長度自適應(yīng)策略。通過滑動采樣區(qū)間長度的實時在線計算，實現(xiàn)系統(tǒng)不同噪聲環(huán)境下漸消因子的求取，解決模型穩(wěn)定情況下滑動采樣區(qū)間過短造成的濾波精度降低和系統(tǒng)模型發(fā)生變化時滑動采樣區(qū)間過長造成的動態(tài)跟蹤性能無法滿足需求的問題，避免滑動采樣區(qū)間固定的弊端，實現(xiàn)濾波精度和動態(tài)性之間的平衡。

圖1 基于IGGIII方案的自適應(yīng)卡爾曼濾波器原理框圖

4 實驗仿真與結(jié)果分析

為驗證改進算法的有效性，本文利用Matlab對二維平面內(nèi)的小車行駛位置估計進行仿真實驗。系統(tǒng)的狀態(tài)方程和量測方程分別為：

式中，狀態(tài)轉(zhuǎn)移矩陣為：

觀測矩陣為：

取位置以及速度作為系統(tǒng)狀態(tài)變量，初始位置為（100，100），且 x軸初始運動速度2 m/s，y軸初始運動速度0 m/s，那么狀態(tài)變量初始值x0=[ ]100 2 100 0。掃描周期為T=1 s，仿真時間100 s?；瑒硬蓸訁^(qū)間長度N在前20 s取k，20 s以后取20。目標小車在前50 s處于平穩(wěn)行駛運動狀態(tài)，在50 s至52 s進行機動。

本文利用以上仿真模型進行了卡爾曼濾波器（KF）、自適應(yīng)漸消卡爾曼濾波器（AFKF）以及基于IGGIII方案的自適應(yīng)漸消卡爾曼濾波器（IGGAFKF）的對比驗證，仿真結(jié)果如圖2所示。

圖2 位置估計結(jié)果

如圖2三種算法的位置估計仿真結(jié)果所示，IGGAFKF的濾波估計結(jié)果更加接近于目標的真實位置。KF和AFKF位置估計都存在不同程度的偏離。KF在目標機動位置出現(xiàn)較大估計偏差，AFKF濾波估計結(jié)果偏離程度要小于KF，但高于IGGAFKF。

圖3分別展示了三種算法的位置誤差對比。通過對比三種算法的誤差曲線可知，在目標穩(wěn)定行進過程中，AFKF的估計誤差存在較大波動，IGGAFKF通過增大滑動采樣區(qū)間長度，增強濾波魯棒性，估計結(jié)果更加平穩(wěn)、精確。在目標進行機動時，KF、AFKF、IGGAFKF誤差都開始變大，但是IGGAFKF誤差峰值明顯低于KF及AFKF，說明IGGAFKF在目標運動狀態(tài)發(fā)生較大變化時對于準確量測信息具有更好的動態(tài)跟蹤性能，能夠抑制模型不準造成的濾波發(fā)散。并且IGGAFKF通過減小滑動采樣區(qū)間長度，在經(jīng)過2次迭代后將誤差控制在1 m范圍內(nèi)，KF和AFKF分別需要8次和4次，所以，IGGAFKF能夠在濾波出現(xiàn)發(fā)散時實現(xiàn)快速收斂，提高濾波精度。由上可知，IGGAFKF在系統(tǒng)穩(wěn)定情況下濾波結(jié)果更加精確，系統(tǒng)模型參數(shù)發(fā)生變化時具有更好的動態(tài)性，能夠?qū)崿F(xiàn)濾波精度與動態(tài)性之間的平衡，驗證了此算法的可行性與優(yōu)越性。

圖3 位置估計誤差對比

圖4 顯示了位置誤差的占比。IGGAFKF 42%的采樣點位置誤差小于0.25 m，89%采樣點位置誤差小于0.5 m，100%的采樣點誤差小于2 m。KF相同誤差的占比分別為38%、77%和89%。AFKF相同誤差的占比為33%、77%和98%。且由表1中三種濾波算法的誤差統(tǒng)計數(shù)據(jù)可知，IGGAFKF最大誤差為1.66 m，明顯小于KF和AFKF最大誤差。IGGAFKF平均誤差為0.32 m，相對于KF與AFKF的平均誤差分別提升了121%、37%。因此，IGGAFKA具有更高的濾波估計精度。

圖4 位置誤差占比

表1 三種濾波算法誤差統(tǒng)計 m

對100組位置仿真數(shù)據(jù)在CPU型號為Intel?CoreTMi5-4210U的計算機上進行10次測試，取耗費時間平均值，得到三種對比算法跑完100組數(shù)據(jù)耗費時間對比表。

由表2中數(shù)據(jù)可以發(fā)現(xiàn)，KF耗時最短，AFKF以及IGGAFKF相較于KF運算耗時都有所增加，但增加幅度較小。將IGGAFKF和AFKF耗時除以100，獲得單組數(shù)據(jù)平均耗時分別為0.513 3 ms和0.468 1 ms，兩者差值僅為0.045 2 ms，濾波效率下降8%。因此，IGGAFKF相較于AFKF濾波效率下降并不明顯，能夠保證濾波器的實時性要求。

表2 100組數(shù)據(jù)耗費時間

5 結(jié)束語

針對滑動采樣區(qū)間長度單一帶來的濾波精度與動態(tài)性之間的矛盾問題，本文提出了一種基于IGGIII方案的漸消自適應(yīng)卡爾曼濾波器。該濾波器算法以新息馬氏距離服從卡方分布作為新息異常判斷標準，并引入IGGIII方案的信息分類思想，對不同噪聲情況下的滑動采樣區(qū)間長度N進行自適應(yīng)優(yōu)化選取。在保證濾波效率的同時，能夠?qū)崿F(xiàn)濾波精度與動態(tài)性之間的平衡。在船舶、飛行器、潛航器等設(shè)備的定位、制導與控制方面具有一定應(yīng)用價值。