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

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

1 引言

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

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

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

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

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

標(biāo)準(zhǔn)卡爾曼濾波器算法濾波過程為[13]：

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

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

則式（3）（8）（5）（6）（7）構(gòu)成了自適應(yīng)漸消卡爾曼濾波算法。漸消因子λk大于等于1，當(dāng)計(jì)算出的漸消因子變?yōu)闃?biāo)準(zhǔn)卡爾曼濾波器[15]。實(shí)際值小于1時(shí)取1，即自適應(yīng)漸消卡爾曼濾波器：

式中，Dk為滑動(dòng)取值長(zhǎng)度為N時(shí)的新息方差估計(jì)值[16]：

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

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

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

3.1 新息異常卡方檢測(cè)

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

式中，Mk為馬氏距離。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

步驟3.1求取新息向量：

步驟3.2通過式（15）進(jìn)行新息卡方檢測(cè)和求取融入IGGIII方案思想的滑動(dòng)區(qū)間長(zhǎng)度。

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

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

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

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

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

步驟7結(jié)束。

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

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

4 實(shí)驗(yàn)仿真與結(jié)果分析

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

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

觀測(cè)矩陣為：

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

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

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

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

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

圖3 位置估計(jì)誤差對(duì)比

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

圖4 位置誤差占比

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

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

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

表2 100組數(shù)據(jù)耗費(fèi)時(shí)間

5 結(jié)束語

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