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

，，

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

0 引言

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

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

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

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

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

(1)

zk=Hkxk+wk

(2)

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

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

(3)

(4)

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

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

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

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

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

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

(5)

為方便描述，定義

(6)

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

(7)

(8)

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

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

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時刻的狀態xk與后驗量測zk+1滿足線性關系(HF)，即量測方程是線性的。此外，后面的噪聲項等于兩個獨立的高斯分布經線性變換后求和，仍然為高斯分布。因此，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 判決準則

工程應用中，一般根據新息大小計算一個檢驗量，用于判斷濾波是否異常。如果正常，則按一般的濾波步驟進行估計，否則，進行適當的修正。判斷濾波異常的判據為[15]：

(17)

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

(18)

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

(19)

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

(20)

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

2.3 修正的卡爾曼濾波算法

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

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

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

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

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

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

3 仿真校驗

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

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

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

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

3.1 仿真實驗一

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

3.2 仿真實驗二

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

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

3.3 仿真實驗三

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

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

3.4 仿真實驗四

本節主要是對所提出的狀態突變的判決準則進行驗證?？烧{白噪聲模型是一種典型的通過門限判決來對濾波器進行調整的機動目標跟蹤算法，因此，可以用來測試門限的優劣。選取連續強機動目標(場景4)，量測噪聲協方差R=diag[100 ,100]，過程噪聲協方差在機動時取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、y誤差之和)。

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

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

1)模型匹配時，通過對濾波器進行微調，提高穩態時的跟蹤精度，加快濾波器的收斂速度。

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

3)與文獻[14]的算法相比，克服了經驗參數的設定，且對突變狀態或模型失配的調節能力更強。

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

4 結論

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

[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].系統工程與電子技術，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]徐定杰，賀瑞，沈鋒，等.基于新息協方差的自適應漸消卡爾曼濾波器[J].系統工程與電子技術，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].系統工程與電子技術，2014，36(5)：846-851.

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