基于后驗量測修正的卡爾曼濾波算法

，，

(解放軍電子工程學院，安徽 合肥 230037)

0 引言

卡爾曼濾波是一種遞推的線性最小均方誤差估計，廣泛應用于自動控制、目標跟蹤、組合導航等領域[1-5]。標準的卡爾曼濾波器是以精確的數(shù)學模型和已知的噪聲統(tǒng)計特性為前提，進行遞推濾波，取得最優(yōu)估計的。但在實際工程中，系統(tǒng)的模型參數(shù)以及噪聲統(tǒng)計特性通常難以獲取，導致卡爾曼濾波精度下降，甚至濾波發(fā)散[6]。此外，卡爾曼濾波器在狀態(tài)收斂時，其誤差協(xié)方差矩陣和增益矩陣趨于極小值，狀態(tài)改變時，不能及時調(diào)整增益矩陣及協(xié)方差矩陣，導致濾波器喪失對突變狀態(tài)的跟蹤能力。

針對標準卡爾曼濾波器的不足，目前的改進方式主要分為兩類：一是建立合適的估計準則，利用新增的量測信息，對模型的參數(shù)和噪聲統(tǒng)計特性進行在線調(diào)整，實現(xiàn)自適應濾波[7-8]；二是把系統(tǒng)模型及噪聲統(tǒng)計特性不準確所帶來的誤差等效為濾波誤差，采用漸消因子抑制濾波器的記憶長度，充分利用當前的量測數(shù)據(jù)，減小陳舊量測的影響，實現(xiàn)漸消濾波[9]。第二類算法僅需要額外增加一個漸消因子的計算，通過該漸消因子對預測協(xié)方差進行修正，間接調(diào)整增益，從而增強濾波器的調(diào)節(jié)能力。由于實現(xiàn)簡單，實用性強，得到了廣泛應用[10-13]。然而，文獻[14]進一步指出，該類算法雖然能較好地解決濾波發(fā)散、增強濾波器對突變狀態(tài)的跟蹤能力，但是仍然存在以下缺陷：1)在模型參數(shù)及噪聲統(tǒng)計特性已知的情況下，漸消因子的引入反而會降低濾波的精度；2)通過漸消因子來增強對當前量測的信任存在修正延后。該文獻將引起卡爾曼濾波器發(fā)散的原因歸結為對狀態(tài)預測值的不準確，提出對狀態(tài)預測值進行實時修正，解決了修正延后的問題。但是該算法需要根據(jù)經(jīng)驗預先設定一系列的修正參數(shù)，存在參數(shù)選取困難。針對上述算法的不足，本文提出了基于后驗量測修正的卡爾曼濾波算法。

1 目標跟蹤建模與修正思路

假設目標的運動狀態(tài)和量測模型均為線性模型，則離散狀態(tài)方程和量測方程可以表示為：

xk=Fk-1xk-1+Γk-1vk-1

(1)

zk=Hkxk+wk

(2)

式中，F(xiàn)k為狀態(tài)轉(zhuǎn)移矩陣；Hk為量測轉(zhuǎn)移矩陣；Γk為過程噪聲分布矩陣；vk、wk為零均值的白色過程噪聲和量測噪聲序列，其協(xié)方差分別為Qk、Rk，且任意時刻vk與wk不相關。由于Fk、Hk、Γk通常為時不變矩陣，故下文統(tǒng)一省略時間下標k，簡記為F、H、Γ。

離散時間卡爾曼濾波器的狀態(tài)預測和狀態(tài)更新方程為：

(3)

(4)

式中，Kk+1為濾波增益矩陣。

2 基于后驗量測修正的卡爾曼濾波算法(PMKF)

2.1 后驗量測卡爾曼濾波(PKF)

為了對卡爾曼濾波器進行修正，本節(jié)提出一種利用后驗量測進行濾波更新的卡爾曼濾波新算法(PKF)，即利用后驗量測zk+1建立對k時刻的狀態(tài)估計。

將量測方程向前反推一步，把式(1)代入式(2)中有：

zk=Hxk+wk=
H(Fxk-1+Γvk-1)+wk=
HFxk-1+HΓvk-1+wk

(5)

為方便描述，定義

(6)

則新的量測方程可以改寫為：

(7)

(8)

由于卡爾曼濾波器是線性濾波器，因此，基于后驗量測改寫后的算法要在卡爾曼濾波器的框架下進行迭代更新，其狀態(tài)方程與量測方程也必須是線性的。下面，對這一點進行說明。

因為PKF對狀態(tài)方程沒有改動，因此只需要證明其量測方程是線性的即可。

zk+1=Hxk+1+wk+1=
H(Fxk+Γvk)+wk+1=
HFxk+HΓvk+wk+1=
(HF)xk+(HΓvk+wk+1)

(9)

由式(9)可知，由于H、F為時不變的常量矩陣，所以，k時刻的狀態(tài)xk與后驗量測zk+1滿足線性關系(HF)，即量測方程是線性的。此外，后面的噪聲項等于兩個獨立的高斯分布經(jīng)線性變換后求和，仍然為高斯分布。因此，PKF與KF一樣，是線性、高斯的，可以在標準的卡爾曼濾波框架下進行濾波更新。

PKF的一個濾波流程為：

步驟1 時間更新

(10)

Pk|k-1=FPk-1|k-1FT+ΓQk-1ΓT

(11)

步驟2 量測更新

(12)

Pk|k=Pk|k-1-Pk|k-1(HNew)T(Sk)-1HNewPk|k-1=

Pk|k-1-Pk|k-1FTHT(Sk)-1HFPk|k-1

(13)

式中：

HFPk|k-1FTHT+HΓQk-1ΓTHT+Rk

(14)

Kk= [Pk|k-1(HNew)T+ΓQk-1ΓTHT](Sk)-1=

(Pk|k-1FTHT+ΓQk-1ΓTHT)(Sk)-1

(15)

2.2 判決準則

工程應用中，一般根據(jù)新息大小計算一個檢驗量，用于判斷濾波是否異常。如果正常，則按一般的濾波步驟進行估計，否則，進行適當?shù)男拚Ｅ袛酁V波異常的判據(jù)為[15]：

(17)

Pk|k-1=FPk-1|k-1FT+Qk-1

(18)

將式(17)，(18)代入式(16)中，取最嚴格的收斂判據(jù)ε=1，可得

(19)

分析式(19)的左邊不難發(fā)現(xiàn)，過程噪聲協(xié)方差Q和量測噪聲協(xié)方差R分別由目標和觀測系統(tǒng)決定，通常不隨目標的運動發(fā)生顯著變化。但是第一項包含反映估計誤差的協(xié)方差矩陣P，在目標狀態(tài)改變時，隨著跟蹤精度下降，P會增大。在目標狀態(tài)持續(xù)性變化，并保持變化強度不變的情況下，P增大，會抬高檢測的門限，使得對突變狀態(tài)的檢測能力被削弱。故本文提出，去掉不等式左邊第一項，判決條件變?yōu)椋?/p>

(20)

上式左邊的物理意義是明確的，為狀態(tài)噪聲和量測噪聲的協(xié)方差之和。也就是說，在目標狀態(tài)不發(fā)生改變的情況下，量測值偏離真實值的最大誤差上限為狀態(tài)噪聲與過程噪聲之和，這正好與實際情況相符。如果量測值偏離的程度超過了最大誤差上限，只能是目標的狀態(tài)已經(jīng)發(fā)生改變。因此，利用式(20)作為目標狀態(tài)改變的判據(jù)更加合理。進一步可以發(fā)現(xiàn)，當目標狀態(tài)持續(xù)性變化時，由于不等式左邊不發(fā)生變化，因此對狀態(tài)突變的檢測能力沒有下降。

2.3 修正的卡爾曼濾波算法

根據(jù)2.1節(jié)、2.2節(jié)提出的PKF算法和判決準則，基于后驗量測修正的卡爾曼濾波算法(PMKF)的修正流程圖如圖1所示，具體步驟如下。

步驟1：利用zk對k-1時刻的狀態(tài)和協(xié)方差進行標準的卡爾曼濾波更新；

步驟2：進行一步預測，并求取k+1時刻的新息向量；

步驟4：若新息超過判決門限，利用后驗量測卡爾曼(PKF)進行濾波更新。否則，直接輸出濾波結果；

步驟5：重復步驟1～4。

該修正方法的優(yōu)勢有兩點：1)當檢測到k+1時刻的目標狀態(tài)發(fā)生改變時，對k時刻的狀態(tài)進行調(diào)整，通過時不變矩陣F的傳遞，達到修正預測值xk+1|k的目的，解決了修正延后的問題。2)對k時刻的估計值進行修正時利用了量測zk+1，而修正之后進行正?？柭鼮V波時，再一次用到量測zk+1，即充分利用了新近量測的信息，起到了類似于漸消因子的作用。

3 仿真校驗

本文采用勻加速模型，傳感器僅提供位置信息。采用均方根誤差評價算法的性能，設置了如下四種仿真場景，各種仿真場景下，Monte-Carlo次數(shù)1 000次。

表1 場景2目標加速度Tab.1 Target acceleration in scene two

表2 場景3目標轉(zhuǎn)彎角速度Tab.2 Target turning angle acceleration in scene three

表3 場景4目標加速度Tab.3 Target acceleration in scene four

3.1 仿真實驗一

從圖中可以看出，文獻[14]和本文算法對位置、速度的跟蹤精度均有較大的提高，但是本文算法更為優(yōu)越，尤其是對位置均方根誤差的改善尤為明顯。然而兩種算法對加速度的改善都不大，其原因在于傳感器只提供了位置信息，通過位置信息的變化修正速度，再修正加速度，其修正作用逐漸減弱。此外，本文算法在初始狀態(tài)的收斂速度比標準的KF以及文獻[14]略快。

3.2 仿真實驗二

利用場景2測試本文算法對加速突變目標的跟蹤能力。過程噪聲協(xié)方差Q=diag[1，1，1]，量測噪聲協(xié)方差R=100，文獻[14]的修正參數(shù)同實驗一。文獻[9]是典型的漸消濾波方法，通過引入漸消因子λ對預測協(xié)方差進行修正，進而調(diào)節(jié)增益矩陣，達到增強濾波器對突變狀態(tài)的跟蹤能力，將其放在相同的場景下進行比較。由于對加速度的改善不明顯，僅給出了標準KF、文獻[14]、文獻[9]以及本文提出的PMKF四種算法的位置及速度估計誤差曲線，如圖5—圖6。

分析圖5、圖6易知，無論是平穩(wěn)狀態(tài)，還是突變狀態(tài)，本文算法的均方根誤差曲線都保持在較低的水平。因此，PMKF對狀態(tài)突變具有較好的適應能力，相比文獻[14]，調(diào)節(jié)能力更強。文獻[9]由于引入了漸消因子，在平穩(wěn)狀態(tài)時的跟蹤精度略低于標準的KF，但是在目標狀態(tài)改變時，跟蹤精度較KF有明顯改善，尤其是變化強度比較大時(體現(xiàn)在最后一次加速度突變)，其跟蹤精度甚至好于文獻[14]，與本文算法相當。

3.3 仿真實驗三

利用場景3測試本文算法在模型不匹配情況下的跟蹤能力。各算法均采用勻加速模型跟蹤二維場景下的勻速和勻轉(zhuǎn)彎混合運動軌跡。過程噪聲協(xié)方差Q=diag[1，1，1，1，1，1]，量測噪聲協(xié)方差R=diag[100 ,100]，文獻[14]的修正參數(shù)不變。圖7—圖8給出了該場景下標準KF、文獻[14]以及PMKF各算法在x軸和y軸的位置均方根誤差。

由圖7、圖8可知，在模型不匹配的情況下，KF的跟蹤精度比較低，尤其是在狀態(tài)突變的時候，有明顯的尖峰。文獻[14]通過實時修正，較好地改善了模型不匹配情況下的跟蹤精度，狀態(tài)突變時也具有較強的適應能力，誤差曲線相對比較平滑。但是本文算法具有更明顯的優(yōu)勢，只在狀態(tài)突變時有小的波動，較好地克服了模型不匹配的影響。

3.4 仿真實驗四

本節(jié)主要是對所提出的狀態(tài)突變的判決準則進行驗證?？烧{(diào)白噪聲模型是一種典型的通過門限判決來對濾波器進行調(diào)整的機動目標跟蹤算法，因此，可以用來測試門限的優(yōu)劣。選取連續(xù)強機動目標(場景4)，量測噪聲協(xié)方差R=diag[100 ,100]，過程噪聲協(xié)方差在機動時取Q=diag[1，1，1，1，1，1]，非機動時取Q=diag[0.005，0.005，0.005，0.005，0.005，0.005]，原始判決門限和本文判決門限分別由式(19)、式(20)給出。圖9—圖10給出了兩種判決門限下對連續(xù)機動目標跟蹤的位置、速度均方根誤差曲線(此處為x、y誤差之和)。

由圖9、圖10易知，本文提出的判決準則在目標發(fā)生連續(xù)機動時，由于能較好地保持對機動的檢測能力，及時調(diào)整濾波器的參數(shù)，一定程度上降低了峰值誤差，而在非機動段取得與原始算法類似的跟蹤精度。因此，本文提出的判決準則更為優(yōu)越。

通過以上的四個仿真實例，本文所提出的基于后驗量測修正的卡爾曼濾波算法(PMKF)主要的優(yōu)勢在于：

1)模型匹配時，通過對濾波器進行微調(diào)，提高穩(wěn)態(tài)時的跟蹤精度，加快濾波器的收斂速度。

2)在狀態(tài)發(fā)生突變或者模型失配時，利用后驗量測進行實時修正，前后兩次用到后驗量測zk+1，增強了對后驗量測的信任度，起到了漸消因子類似的調(diào)節(jié)作用。

3)與文獻[14]的算法相比，克服了經(jīng)驗參數(shù)的設定，且對突變狀態(tài)或模型失配的調(diào)節(jié)能力更強。

4)在狀態(tài)連續(xù)變化時，所提出的判決準則能保持對狀態(tài)突變的檢測能力，及時修正濾波器，從而取得更高的濾波精度。

4 結論

本文提出了基于后驗量測修正的卡爾曼濾波算法。該算法通過對濾波狀態(tài)進行實時判決，狀態(tài)平穩(wěn)時采用卡爾曼濾波，狀態(tài)突變時采用后驗量測卡爾曼濾波算法進行修正。理論分析及仿真結果表明，改進的判決準則能保持對連續(xù)突變狀態(tài)的檢測，而后驗量測的修正，不僅提高了穩(wěn)態(tài)時的跟蹤精度，削弱了狀態(tài)突變帶來的影響，克服了模型失配的缺陷，而且不需要預設修正參數(shù)，簡單實用，具有一定的工程應用價值。由于卡爾曼濾波器的輸出結果由預測項和修正項兩部分組成，實際工程應用中完全可以既對預測項進行修正，也對修正項進行調(diào)整，以取得更好的濾波結果。如何將二者進行合理融合，形成優(yōu)勢互補是下一步要討論和研究的課題。

[1]RIGATOS G, SIANO P. Control of heat diffusion in arc welding using differential flatness theory and nonlinear kalman filtering [J]. Intelligent Industrial Systems, 2016, 2(1): 5-19.

[2]YU Y. Consensus-Based distributed mixturekalman filter for maneuvering target tracking in wireless sensor networks [J]. IEEE Transactions on Vehicular Technology, 2016, 65(10): 8669-8681.

[3]ZHU W, WANG W, YUAN G. An improved interacting multiple model filtering algorithm based on the cubature kalman filter for maneuvering target tracking [J]. Sensors, 2016, 16(6): 805.

[4]ZHAO Y. Performance evaluation of CubatureKalman filter in a GPS/IMU tightly-coupled navigation system [J]. Signal Processing, 2016, 119(C): 67-79.

[5]周啟帆，張海，王嫣然.一種基于冗余測量的自適應卡爾曼濾波算法[J].航空學報，2015，36(5)：1596-1605.

[6]高偉，李敬春，奔粵陽，等.基于多重漸消因子的自適應卡爾曼濾波器[J].系統(tǒng)工程與電子技術，2014(7)：1405-1409.

[7]SAGE A P, HUSA G W. Adaptive filtering with unknown prior statistics [C] //Proceedings of Joint Automatic Control Conference. US: University of Colorado, 1969: 760-769.

[8]XU T H, NAN J, SUN ZZ. An improved adaptive Sage filter with applications in GEO orbit determination and GPS kinematic positioning[J]. Science China Physics, Mechanics & Astronomy, 2012, 55(5): 892-898.

[9]徐定杰，賀瑞，沈鋒，等.基于新息協(xié)方差的自適應漸消卡爾曼濾波器[J].系統(tǒng)工程與電子技術，2011，33(12)：2696-2699.

[10]XIA Q, RAO M, YING Y, et al. Adaptive fadingkalman filter with an application [J]. Automatica, 1994, 30(8): 1333-1338.

[11]柯晶，錢積新.基于邏輯切換的改進強跟蹤卡爾曼濾波器[J].電子學報，2002，30(6)：925-927.

[12]劉玉磊，馮新喜，鹿傳國，等.帶漸消因子的Quadrature卡爾曼濾波[J].宇航學報，2013，34(10)：1370-1377.

[13]馬云峰.改進的強跟蹤卡爾曼濾波器在MSINS/GPS組合導航中的應用研究[J].濰坊學院學報，2014，14(2)：1-5.

[14]楊永建，樊曉光，王晟達，等.基于修正卡爾曼濾波的目標跟蹤[J].系統(tǒng)工程與電子技術，2014，36(5)：846-851.

[15]徐景碩，秦永元，彭蓉.自適應卡爾曼濾波器漸消因子選取方法研究[J].系統(tǒng)工程與電子技術，2004，26(11)：1552-1554.