基于IGGIII 方案的自適應漸消卡爾曼濾波器

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

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

1 引言

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

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

2 自適應漸消卡爾曼濾波器

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

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

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

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

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

則式（3）（8）（5）（6）（7）構成了自適應漸消卡爾曼濾波算法。漸消因子λk大于等于1，當計算出的漸消因子變為標準卡爾曼濾波器[15]。實際值小于1時取1，即自適應漸消卡爾曼濾波器：

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

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

3 基于IGGIII方案的自適應漸消卡爾曼濾波器

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

3.1 新息異常卡方檢測

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

式中，Mk為馬氏距離。

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

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

3.2 IGGIII權函數

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

式中， ||ui為標準化殘差，k0和k1為調和系數。

3.3 滑動采樣區間長度優化選取

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

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

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

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

結合新息卡方異常檢測和滑動采樣區間長度優化選取的內容，本文提出的基于IGGIII方案的自適應漸消卡爾曼濾波器算法工作流程如下：

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

步驟2通過式（3）進行一步狀態預測。

步驟3求取自適應漸消因子。

步驟3.1求取新息向量：

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

步驟3.3求取新息方差最優估計值：

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

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

步驟5通過式（5）～（7）計算卡爾曼增益、狀態最優估計以及系統誤差協方差，進行濾波更新。

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

步驟7結束。

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

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

4 實驗仿真與結果分析

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

式中，狀態轉移矩陣為：

觀測矩陣為：

取位置以及速度作為系統狀態變量，初始位置為（100，100），且 x軸初始運動速度2 m/s，y軸初始運動速度0 m/s，那么狀態變量初始值x0=[ ]100 2 100 0。掃描周期為T=1 s，仿真時間100 s。滑動采樣區間長度N在前20 s取k，20 s以后取20。目標小車在前50 s處于平穩行駛運動狀態，在50 s至52 s進行機動。

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

圖2 位置估計結果

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

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

圖3 位置估計誤差對比

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

圖4 位置誤差占比

表1 三種濾波算法誤差統計 m

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

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

表2 100組數據耗費時間

5 結束語

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