葛寶爽，張 海，唐志坤

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

0 引言

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

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

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

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

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為相互獨立的高斯白噪聲。

(2)

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

2 改進抗差自適應卡爾曼濾波

2.1 改進Sage-Husa自適應量測噪聲方差估計

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

(3)

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

(4)

2.2 改進自適應濾波穩(wěn)定性分析

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

(5)

(6)

2.3 改進抗差估計策略

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

由多元正態(tài)分布的基本性質(zhì)可知p元正態(tài)分布是p個相互獨立的一維正態(tài)變量的聯(lián)合分布，據(jù)此可依據(jù)新息服從多元正態(tài)分布的特性對量測異常進行逐維檢測判別。另受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維新息標準差。K(:,i)=0表示將卡爾曼增益陣的第i列元素置零。

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

2.4 改進抗差自適應濾波方案

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

圖1 改進抗差自適應濾波方案Fig.1 Improved robust adaptive filter scheme

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

3 仿真驗證

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

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

Xk=

(8)

仿真實驗1中，在x軸位置量測20～70歷元間，每隔5個歷元疊加均值為10，方差為64的孤立型異常值。仿真實驗2中，在x軸位置量測50～70歷元之間，加入均值為0，方差為64的連續(xù)型異常干擾。針對以上兩種情況，分別進行100次Monte Carlo實驗，仿真結(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)可知，當出現(xiàn)孤立型量測異常時，IRAKF能夠有效抑制異常值對濾波結(jié)果的影響，說明所提算法能夠提高濾波精度且具有一定的魯棒性。SKF濾波結(jié)果產(chǎn)生了較大的偏差，這主要是因為其不具有抵抗量測異常的能力。RRAE算法可以有效抵制孤立型異常的干擾，并且具有自適應調(diào)節(jié)量測噪聲方差陣的能力。進一步對比IRAKF與RRAE濾波的RMSE，前者位置RMSE為1.5942m，后者位置RMSE為2.1969m，IRAKF位置精度提高了27.43%。RAKF受量測異常影響嚴重，這主要是因為在RAKF算法中更加依賴量測值。雖然RSSHAKF能夠在一定程度上抑制量測異常的干擾，但較IRAKF其效果有待提高，RSSHAKF位置RMSE為2.1818m。由圖2(c)和圖2(d)可知，在未有量測異常時，除RRAE算法波動較大外，其余算法濾波效果相當。

圖3中僅給出了有量測異常發(fā)生時的濾波誤差圖。由圖3可知，當連續(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ā)散的風險。

(a) X軸位置濾波誤差

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

4 結(jié)論

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

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

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

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