葛寶爽，張 海，唐志坤

(1.北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京 100191；2.國家空域管理中心，北京 100094)

0 引言

卡爾曼濾波作為一線性最優(yōu)估計，被廣泛地應(yīng)用到導(dǎo)航系統(tǒng)與目標(biāo)跟蹤系統(tǒng)中。在狀態(tài)空間模型是線性且噪聲統(tǒng)計特性完全已知并為相互獨(dú)立的高斯白噪聲時，卡爾曼濾波可以得到在最小均方意義下的最優(yōu)解。然而在工程實(shí)踐中，由于外界環(huán)境的干擾，量測噪聲統(tǒng)計特性通常是未知或時變的，在此情況下將無法直接使用標(biāo)準(zhǔn)卡爾曼濾波(Standard Kalman Filter, SKF)；另外，量測中不免會含有異常值，由于SKF自身不具有抵抗異常值的能力，這就導(dǎo)致盡管很少的量測異常便會引起濾波振蕩，致使濾波精度下降[1]。

針對工程實(shí)踐中面臨的量測不穩(wěn)定問題，SKF有不同的改進(jìn)方案，如：適用于模型參數(shù)或噪聲統(tǒng)計特性未知的自適應(yīng)卡爾曼濾波(Adaptive Kalman Filter, AKF)[2-3]；結(jié)合抗差估計理論的抗差濾波[4-5]等。在卡爾曼濾波中，新息作為濾波中的關(guān)鍵量，含有量測的全部信息，并且對異常量測值極為敏感。因此，無論是在AKF還是抗差濾波中，經(jīng)常利用新息的特性估計量測噪聲方差或異常檢測，以期在保證濾波精度的前提下提高算法的魯棒性與容錯性。

文獻(xiàn)[6-7]利用新息正交特性對量測中的異常值進(jìn)行辨識，然后利用抗差估計理論中的Huber方案構(gòu)造等價權(quán)函數(shù)(活化函數(shù))，并引入到卡爾曼濾波算法中，該活化函數(shù)通過降低量測異常值的權(quán)重提高了組合導(dǎo)航系統(tǒng)的抗差性能。文獻(xiàn)[8-9]依據(jù)新息服從多維正態(tài)分布的特性，采用3σ準(zhǔn)則對量測異常值進(jìn)行辨識，隨后通過調(diào)節(jié)濾波增益陣減弱異常值對濾波結(jié)果的影響。文獻(xiàn)[10-11]依據(jù)概率統(tǒng)計知識，構(gòu)造新息的卡方統(tǒng)計量，并成功地應(yīng)用到目標(biāo)跟蹤與組合導(dǎo)航的異常值檢測中。雖然文獻(xiàn)[10]從理論上證明了所構(gòu)造的等價權(quán)函數(shù)能夠保持新息的特性不變，但在實(shí)際應(yīng)用中因其需要計算矩陣平方根的逆，存在數(shù)值穩(wěn)定性問題；文獻(xiàn)[11]將Huber方案構(gòu)造的等價權(quán)因子與卡方檢驗(yàn)的臨界值相結(jié)合構(gòu)造了新的方差膨脹因子，當(dāng)量測異常發(fā)生時，通過方差膨脹因子放大量測噪聲方差陣降低了異常量測權(quán)重，因此增強(qiáng)了濾波的抗差性能。文獻(xiàn)[12]將AKF與抗差估計理論相結(jié)合，利用抗差估計理論構(gòu)造等價權(quán)因子的方法構(gòu)造了AKF中的自適應(yīng)調(diào)節(jié)因子，并利用新息構(gòu)成的統(tǒng)計量實(shí)時檢測調(diào)節(jié)量測等價權(quán)矩陣，以平衡狀態(tài)預(yù)測與量測間的權(quán)重。文獻(xiàn)[13]在保留移動開窗法估計量測噪聲方差陣的同時，引入了自適應(yīng)因子來調(diào)節(jié)量測異常值對整個濾波系統(tǒng)的影響，較SKF提高了濾波精度且增強(qiáng)了濾波穩(wěn)定性。

以上有關(guān)卡爾曼濾波改進(jìn)算法的研究具有一定的理論意義和工程實(shí)踐應(yīng)用價值，并且能夠較好地應(yīng)對量測噪聲統(tǒng)計特性未知及量測中的孤立型異常值。但是，量測中連續(xù)型異常值的出現(xiàn)嚴(yán)重制約了上述算法的應(yīng)用范圍與濾波效果。為此，本文首先從新息的角度分析了量測異常對濾波結(jié)果的影響；隨后，針對工程實(shí)踐中量測噪聲統(tǒng)計特性未知且含有連續(xù)型異常值的情形，構(gòu)造了魯棒性更強(qiáng)的自適應(yīng)Sage-Husa濾波算法；通過仿真實(shí)驗(yàn)比較了改進(jìn)算法與常規(guī)抗差自適應(yīng)濾波算法的性能，驗(yàn)證了改進(jìn)算法抑制量測異常的有效性。

1 卡爾曼濾波量測異常影響分析

考慮量測中含有異常值的離散狀態(tài)方程為

Xk=Φk,k-1Xk-1+Wk-1
Zk=HkXk+bk+Vk

(1)

式中，Xk為狀態(tài)量，Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣，Zk為量測量，Hk為量測矩陣,bk為異常量測，Wk-1與Vk為相互獨(dú)立的高斯白噪聲。

(2)

當(dāng)bk=0時，式(2)便退化為SKF；當(dāng)bk≠0時，即量測中含有異常值時，由式(2)中的新息定義可知，量測中的異常值必然引起新息的異常，受異常值污染的新息不僅會使當(dāng)前時刻的濾波精度下降，同時還將會通過一步預(yù)測值向后續(xù)時刻傳遞，引起濾波振蕩。對于連續(xù)型異常值，若不采取必要的檢測和修復(fù)算法則很可能導(dǎo)致濾波最終發(fā)散，不能滿足導(dǎo)航或目標(biāo)跟蹤的定位要求。另一方面，由式(1)中受異常污染的量測方程可以直接看出，若異常bk表現(xiàn)為連續(xù)型隨機(jī)噪聲的形式則可看作為量測量新增加性噪聲，因此會引起量測噪聲統(tǒng)計特性的變化。

2 改進(jìn)抗差自適應(yīng)卡爾曼濾波

2.1 改進(jìn)Sage-Husa自適應(yīng)量測噪聲方差估計

Sage-Husa自適應(yīng)濾波算法通過對新息序列或殘差序列進(jìn)行開窗估計出當(dāng)前時刻的量測噪聲方差陣[14]。在基于新息與殘差的開窗估計算法中，基于殘差的自適應(yīng)估計(Residual-based Adaptive Estimation, RAE)算法避免了基于新息的自適應(yīng)估計算法估計量測方差陣時可能出現(xiàn)的負(fù)定現(xiàn)象。但RAE在估計量測噪聲方差陣Rk時需要用到當(dāng)前時刻的狀態(tài)估計方差陣Pk，而計算Pk又需要已知Rk。為了避免上述矛盾，常規(guī)做法是采用上一時刻的狀態(tài)估計方差陣Pk-1來代替Pk，如式(3)所示

(3)

考慮量測異常的影響，本文采用模值更大的預(yù)測方差陣Pk/k-1來代替原算法中的Pk-1陣，對Rk估計時起到放大作用，以降低異常量測權(quán)重。改進(jìn)后的估計算法如式(4)所示

(4)

2.2 改進(jìn)自適應(yīng)濾波穩(wěn)定性分析

改進(jìn)后的Sage-Husa自適應(yīng)濾波在估計量測噪聲方差時采用了當(dāng)前時刻的預(yù)測方差陣Pk/k-1。為了證明改進(jìn)后的自適應(yīng)濾波算法仍然是濾波穩(wěn)定的，首先給定如下假設(shè)條件：原系統(tǒng)可控、可觀且對任意時刻k>0有

(5)

(6)

2.3 改進(jìn)抗差估計策略

盡管Sage-Husa自適應(yīng)濾波算法可以對時變噪聲方差進(jìn)行在線估計，但其對量測中的異常并無抵制能力。為此可將抗差估計的思想引入到Sage-Husa濾波中以構(gòu)造具有抗差能力的自適應(yīng)濾波算法。

由多元正態(tài)分布的基本性質(zhì)可知p元正態(tài)分布是p個相互獨(dú)立的一維正態(tài)變量的聯(lián)合分布，據(jù)此可依據(jù)新息服從多元正態(tài)分布的特性對量測異常進(jìn)行逐維檢測判別。另受IGG函數(shù)與Huber函數(shù)的啟發(fā)，構(gòu)造如下權(quán)函數(shù)

(7)

式中，c0可取(0.8～1.5)σi，c1可取(3.0～5.0)σi，σi為第i維新息標(biāo)準(zhǔn)差。K(:,i)=0表示將卡爾曼增益陣的第i列元素置零。

由式(7)可知，抗差權(quán)函數(shù)的降權(quán)區(qū)采用了較為簡潔的Huber函數(shù)降權(quán)形式。拒絕區(qū)則不同于原IGG函數(shù)直接將新息置零，而是將加權(quán)異常新息的濾波增益置零，由此可在抑制新息異常的同時調(diào)諧狀態(tài)估計方差陣。改進(jìn)的拒絕區(qū)避免了連續(xù)異常出現(xiàn)時新息持續(xù)置零，但未相應(yīng)調(diào)節(jié)狀態(tài)估計方差陣致使其不能反映當(dāng)前時刻受異常影響濾波估值的可信度，最終導(dǎo)致濾波發(fā)散的情況。

2.4 改進(jìn)抗差自適應(yīng)濾波方案

改進(jìn)抗差自適應(yīng)卡爾曼濾波(Improved Robust Adaptive Kalman Filter, IRAKF)算法流程如圖1所示。

圖1 改進(jìn)抗差自適應(yīng)濾波方案Fig.1 Improved robust adaptive filter scheme

圖1中，C為閾值因子。由圖1可知，改進(jìn)的抗差自適應(yīng)濾波算法分為兩部分。第一部分為抗差Sage-Husa自適應(yīng)濾波，在開窗估計量測噪聲方差的同時,利用所構(gòu)造的三段權(quán)函數(shù)依據(jù)新息的不同幅值對其施加權(quán)重以控制量測異常的影響；另外，為了避免因開窗估計調(diào)節(jié)量測噪聲方差引起漏檢率升高，第二部分采用SKF中的新息輔助量測異常檢測，當(dāng)檢測出量測異常時，直接通過軟件方式實(shí)現(xiàn)卡爾曼濾波增益陣列置零。經(jīng)過雙重檢測可以有效地控制量測異常對濾波結(jié)果的影響。

3 仿真驗(yàn)證

為了能夠驗(yàn)證本文所提IRAKF算法對不同類型量測異常的抑制能力，本文以目標(biāo)跟蹤為例進(jìn)行了仿真驗(yàn)證實(shí)驗(yàn)，并與SKF及現(xiàn)有抗差自適應(yīng)濾波算法如基于殘差的抗差自適應(yīng)濾波(Robust Residual-based Adaptive Estimation, RRAE)[16]、自適應(yīng)抗差卡爾曼濾波(Robust Adaptive Kalman Filter, RAKF)[17]、簡化抗差Sage-Husa自適應(yīng)濾波(Robust Simplified Sage-Husa Adaptive Kalman Filter, RSSHAKF)[18]算法的濾波精度作了比較。

在仿真實(shí)驗(yàn)中，系統(tǒng)狀態(tài)量為二維位置與速度，即X=[x,Vx,y,Vy]T，狀態(tài)初值設(shè)為X0=[1.25×105m,-100m, 1.25×105m,-100m]T。狀態(tài)空間方程如下

Xk=

(8)

仿真實(shí)驗(yàn)1中，在x軸位置量測20～70歷元間，每隔5個歷元疊加均值為10，方差為64的孤立型異常值。仿真實(shí)驗(yàn)2中，在x軸位置量測50～70歷元之間，加入均值為0，方差為64的連續(xù)型異常干擾。針對以上兩種情況，分別進(jìn)行100次Monte Carlo實(shí)驗(yàn)，仿真結(jié)果如圖2和圖3所示。濾波結(jié)果均方根誤差(Root Mean Square Error, RMSE)定義如下

(9)

(a) X軸位置濾波誤差

(b) X軸速度濾波誤差

(c) Y軸位置濾波誤差

(d) Y軸速度濾波誤差圖2 孤立型量測異常濾波誤差Fig.2 Filtering errors with isolated outliers in measurement

由圖2(a)和圖2(b)可知，當(dāng)出現(xiàn)孤立型量測異常時，IRAKF能夠有效抑制異常值對濾波結(jié)果的影響，說明所提算法能夠提高濾波精度且具有一定的魯棒性。SKF濾波結(jié)果產(chǎn)生了較大的偏差，這主要是因?yàn)槠洳痪哂械挚沽繙y異常的能力。RRAE算法可以有效抵制孤立型異常的干擾，并且具有自適應(yīng)調(diào)節(jié)量測噪聲方差陣的能力。進(jìn)一步對比IRAKF與RRAE濾波的RMSE，前者位置RMSE為1.5942m，后者位置RMSE為2.1969m，IRAKF位置精度提高了27.43%。RAKF受量測異常影響嚴(yán)重，這主要是因?yàn)樵赗AKF算法中更加依賴量測值。雖然RSSHAKF能夠在一定程度上抑制量測異常的干擾，但較IRAKF其效果有待提高，RSSHAKF位置RMSE為2.1818m。由圖2(c)和圖2(d)可知，在未有量測異常時，除RRAE算法波動較大外，其余算法濾波效果相當(dāng)。

圖3中僅給出了有量測異常發(fā)生時的濾波誤差圖。由圖3可知，當(dāng)連續(xù)型異常量測發(fā)生時，IRAKF仍能夠有效抑制，其位置RMSE為1.4663m，速度RMSE為0.3367m/s。SKF、RAKF與RSSHAKF濾波位置最大誤差均超過了10m，且在50～70歷元間持續(xù)振蕩。雖然RRAE濾波效果優(yōu)于除IRAKF外的其他算法，其位置RMSE為1.9583m，速度RMSE為0.5475m/s，但其存在濾波發(fā)散的風(fēng)險。

(a) X軸位置濾波誤差

(b) X軸速度濾波誤差圖3 連續(xù)型量測異常濾波誤差Fig.3 Filtering errors with continuous outliers in measurement

4 結(jié)論

本文針對卡爾曼濾波中量測異常的問題，提出了一種改進(jìn)的抗差自適應(yīng)濾波方案。算法分析與實(shí)驗(yàn)結(jié)果表明：

1)量測異常通過污染新息進(jìn)一步影響濾波結(jié)果。本文所提出的改進(jìn)濾波方案采用雙重檢測方式，通過新息統(tǒng)計量檢測量測異常，提高了異常檢測的有效性。

2)新構(gòu)建的量測異常抑制權(quán)函數(shù)合理分配權(quán)重，有效控制了量測異常對整個濾波過程的影響，提高了濾波精度。

3)本文所提改進(jìn)抗差自適應(yīng)濾波方案，因采用了雙濾波結(jié)構(gòu)，因此在一定程度上增加了計算量。對一些對計算量有嚴(yán)格要求的工程應(yīng)用，可只采用抗差Sage-Husa自適應(yīng)部分以減少計算量。