欠觀測條件下的增量容積卡爾曼濾波

馬麗麗,趙甜甜,陳金廣,2

(1.西安工程大學計算機科學學院,西安710048;2.西安電子科技大學電子工程學院,西安710071)

欠觀測條件下的增量容積卡爾曼濾波

馬麗麗1,趙甜甜1,陳金廣1,2

(1.西安工程大學計算機科學學院,西安710048;2.西安電子科技大學電子工程學院,西安710071)

在工程實踐中,由于環境影響、量測設備不穩定等因素,非線性濾波系統中的量測方程可能會出現較大的系統誤差,而標準的非線性濾波算法不能消除這類系統誤差。針對該問題,假定過程噪聲和量測噪聲服從高斯分布,利用相鄰量測時刻的量測值之差建立增量量測方程,采用3階球面徑向規則獲得容積點及其權值。使用容積點對貝葉斯濾波過程中的積分進行數值近似,從而提出增量容積卡爾曼濾波算法。仿真實驗結果表明,增量容積卡爾曼濾波算法濾波精度優于標準容積卡爾曼濾波算法與增量卡爾曼濾波算法,能夠成功消除量測方程中的系統誤差。

增量量測方程;增量容積卡爾曼濾波;欠觀測條件;卡爾曼濾波;狀態估計;深空探測

1 概述

濾波就是在對系統可觀測信號進行量測的基礎上,根據某種估計準則,對系統的狀態進行估計的理論和方法。應用于線性隨機系統的最優估計即是卡爾曼濾波,對于非線性隨機系統,可以對非線性函數進行泰勒級數展開進行線性化近似,再利用卡爾曼濾波算法進行處理,這就是擴展卡爾曼濾波算法。這些方法廣泛應用于信號處理、導航制導等領域,并獲得了巨大的成功[1]。為了獲得更精確的濾波性能,出現了不敏卡爾曼濾波[2]、求積分卡爾曼濾波[3]、中心差分卡爾曼濾波[4]、粒子濾波[5]、容積卡爾曼濾波(Cubature Kalman Filtering,CKF)[6]等方法,這些方法都能夠有效地在一定程度上提高濾波的精度。其中,粒子濾波算法濾波精度最高,但是計算量很大。近年來出現的容積卡爾曼濾波算法計算量較小而且濾波精度較高,受到了研究人員的關注。文獻[7]針對系統噪聲為加性的情況,結合擴維的思想提出了擴維容積卡爾曼濾波,能夠進一步提高濾波精度。文獻[8]提出了均方根容積信息濾波器,能夠提高容積卡爾曼濾波器的數值穩定性和計算效率。針對連續離散系統,文獻[9]提出了連續離散容積卡爾曼濾波器,對標準容積卡爾曼濾波器進行了擴展,使其適用于動態方程為連續型而量測方程為離散型的情況。針對只測角跟蹤系統,文獻[10]將容積卡爾曼濾波器融入到高斯和濾波框架中,能夠獲得較好的濾波性能。

然而,在一些應用中,例如深空探測,由于環境因素的影響,量測設備的不穩定性、模型和參數的選取不當等因素,造成量測數據中的系統誤差隨時間變化而漂移。此時,標準的濾波算法不能夠消除量測系統誤差。為了解決該問題,針對線性系統,文獻[11]提出了增量Kalman濾波算法,該算法能夠成功消除未知的量測系統誤差,大大提高了卡爾曼濾波的精度。針對非線性系統,提出了相應的擴展增量卡爾曼濾波[12]、無跡增量卡爾曼濾波[13]、增量粒子濾波等算法[14]。

為進一步提高增量濾波算法的精度,本文提出增量容積卡爾曼濾波算法,為增量濾波方法提供了新的實現途徑。

2 問題描述

假定具有加性噪聲的非線性系統如下:

其中,xk表示狀態向量;zk表示觀測向量;f和h分別表示非線性狀態轉移函數和量測函數;ak表示由于量測方程建模與實際量測模型之間存在的未知系統誤差;wk-1和vk分別表示過程噪聲和量測噪聲,均值為0。通常假設噪聲分量滿足高斯分布,且:

若直接采用容積卡爾曼濾波算法進行濾波,則由于式中存在未知系統誤差分量ak,使得濾波結果誤差較大,甚至發散。為了解決該問題,令:

若采樣足夠密集,則ak-ak-1→0,可得:

增量量測方程中的隨機噪聲分量Vk=vk-vk-1,由于2個高斯分布的隨機變量之和(差)仍然服從高斯分布,且均值為2個隨機變量均值之和(差),協方差為:

此外,由式(3)知,相鄰時刻的量測噪聲分量vk和vk-1不相關。因此,隨機噪聲分量Vk的方差為:

式(5)即為增量量測方程,增量量測方程成功地消除了原來量測方程中存在的未知系統誤差。

3 增量容積卡爾曼濾波

假定過程噪聲服從高斯分布,根據貝葉斯濾波理論,狀態估計的時間更新計算公式為:

其中,Rn表示n維實數空間;ΔZ1:k-1表示從第1時刻直到第k-1時刻所有增量量測值的集合;N(·)表示高斯分布。相應地,時間更新的協方差計算公式推導為:

假設預測量測的誤差是零均值白噪聲序列,通常假定其為高斯分布,則似然密度函數可以寫為:

其中,預測增量量測值為:

相應的協方差矩陣為:

其中,狀態向量和增量量測向量的協方差矩陣計算式為:

一旦接收到k時刻的量測值zk,則可以計算出該時刻的增量量測值Δzk,根據貝葉斯濾波方程計算狀態向量的后驗分布,即:

其中:

在噪聲分布為高斯分布的假定下,貝葉斯濾波理論中的時間更新和量測更新過程中存在非線性函數和高斯概率密度函數乘積的積分形式,如式(6)、式(7)、式(9)、式(10)和式(12)所示,其中存在的關鍵問題就是要找出該積分的數值解法。下面介紹容積點積分方法。

考慮一個n維加權積分:

其中,f(x)是一個非線性函數;權值函數ω(x)是一個高斯密度函數,且對于所有x∈D,D為n維實數域,有ω(x)≥0。對于這類高斯加權積分可以采用數值積分的方法進行近似,即找出一組具有權值ωi的點集xi,通過加權求和的方式對積分I(f)進行近似:

在容積點積分方法中,使用球面徑向規則獲得一組點集及其權值。對于3階球面徑向規則,總共需要2n個點,這些點被稱為容積點,其中,n表示狀態向量的維數。根據球面徑向規則,計算一個標準高斯加權積分的數值化近似公式為:

而[1]i表示對應序列中的第i個單位點。

對于式(1)和式(5)所表示的非線性增量系統,假設噪聲分布服從高斯模型,可以采用容積點的方法計算貝葉斯濾波過程中的積分。假定狀態向量初始值及其協方差0|0,P0|0已知,則根據k-1時刻的狀態估計k-1|k-1及其協方差Pk-1|k-1,可得出k時刻的狀態估計k|k及其協方差Pk|k。遞推過程的時間更新步驟為:

Step 1 采用Cholesky或者奇異值方法分解協方差:

Step 2 根據式(20)中容積點χi的定義,計算容積點(i=1,2,…,m):

Step 3 根據式(20)計算預測狀態容積點:

Step 4 估計預測狀態:

Step 5 估計預測協方差:

遞推過程的量測更新步驟為:

Step 1 對預測協方差矩陣進行分解:

Step 2 計算容積點(i=1,2,…,m):

Step 3 根據式(5)計算相應的增量量測容積點:

Step 4 估計預測增量:

Step 5 估計增量量測的新息協方差矩陣:

Step 6 估計互協方差矩陣:

Step 7 利用式計算卡爾曼濾波增益,利用式(14)和式(15)估計狀態更新及其協方差矩陣。

4 仿真實驗及結果分析

考慮單變量非穩態增長模型的強非線性系統[5]:

其中,wk和vk是k時刻不相關的過程噪聲分量和量測噪聲分量,分別服從均值為0方差為Q和R的高斯分布。在實驗中取Q=1,R=1,初始狀態x0|0=0,相應的初始協方差為P0|0=10。量測方程的系統誤差ak=4為未知量。

為了使用標準容積卡爾曼濾波算法,忽略掉量測方程中未知的不確定分量ak,即假定ak=0。由于

因此增量量測方程中消去了量測系統的未知分量,此時采用增量容積卡爾曼濾波算法可以獲得更好的濾波精度。為方便起見,假定vk和vk-1不相關,則增量量測方程中的噪聲分量Vk服從高斯分布,且其均值為0,方差為2R。

分別采用標準容積卡爾曼濾波算法和增量容積卡爾曼濾波算法進行仿真,濾波結果如圖1和圖2所示。

圖1 狀態估計

由圖1可知,增量容積卡爾曼濾波與標準容積卡爾曼濾波相比,精確度更高。圖2是2種方法狀態估計結果和真實結果之差。增量容積卡爾曼濾波的誤差聚集于0值附近,而標準容積卡爾曼濾波誤差幅度則較大。并不是任意時刻中的增量容積卡爾曼濾波誤差都比標準容積卡爾曼濾波誤差低,例如,在圖2誤差曲線中的第9步和第88步,本文算法的誤差反而比較大。但是從整個結果來看,增量容積卡爾曼濾波比標準容積卡爾曼濾波濾波精度高。

圖2 狀態誤差

采用增量容積卡爾曼濾波、標準容積卡爾曼濾波、增量擴展卡爾曼濾波和標準擴展卡爾曼濾波對上述例子重復運行10 000次,計算各種算法狀態估計的均方根誤差,其值依次為8.405 9,10.069 3, 10.734 4和12.5135?？芍隽咳莘e卡爾曼濾波誤差低于標準容積卡爾曼濾波,增量擴展卡爾曼濾波誤差低于標準擴展卡爾曼濾波,標準容積卡爾曼濾波誤差略低于增量擴展卡爾曼濾波。上述結果表明,增量容積卡爾曼濾波能夠成功消除量測方程中的系統誤差,與增量卡爾曼濾波相比,能夠獲得更高的濾波精度。

5 結束語

本文在濾波系統的高斯假設下,結合3階球面徑向積分規則,提出了一種新的非線性增量濾波算法——增量容積卡爾曼濾波算法。該算法能夠成功消除量測系統誤差,濾波精度不但高于標準容積卡爾曼濾波,而且還高于增量擴展卡爾曼濾波算法。然而,由于算法中的容積點個數是狀態向量維數的2倍,因此運算時間復雜度與增量擴展卡爾曼濾波算法相比有所提高。新算法濾波精度的提高是以時間復雜度為代價換來的。在通常情況下,容積點的選取采用3階球面徑向積分規則。如果采用高階積分規則,使用更多的積分點,理論上可以獲得更高的濾波精度,關于高階容積卡爾曼濾波算法誤差性能的研究是下一步需要探討的課題。

[1] 韓崇昭,朱洪艷,段戰勝.多源信息融合[M].2版.北京:清華大學出版社,2010.

[2] Julier S J,Uhlmann J K.UnscentedFiltering and Nonlinear Estimation[J].Proceedings of the IEEE, 2004,92(3):401-422.

[3] Arasaratnam I,Haykin S.Discrete-time Nonlinear Filtering Algorithms Using Gauss-Hermite Quadrature [J].Proceedings of the IEEE,2007,95(5):953-977.

[4] Norgaard M,Poulsen N K,Ravn O.NewDevelopments in State Estimation for Nonlinear System[J]. Automatica,2000,36(11):1627-163.

[5] Arulampalam M S,Maskell S,Gordon N,et al.A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking[J].IEEE Transactions on Signal Processing,2002,50(2):174-188.

[6] Arasaratnam I,Haykin S.Cubature KalmanFilters[J]. IEEE Transactions on Automatic Control,2009,54(6): 1254-1269.

[7] Li Pengfei,Yu Jianping,Wang Mingjie,et al.The AugmentedForm ofCubatureKalman Filterand Quadrature Kalman Filter for Additive Noise[C]// Proc.ofIEEE Youth Conferenceon Information, Computing andTelecommunication.[S.l.]:IEEE Press,2009:295-298.

[8] Chandra K P B,Gu Dawei,Postlethwaite I.Square Root Cubature Information Filter[J].IEEE Sensors Journal, 2013,13(2):750-758.

[9] Arasaratnam I,Haykin S,Hurd R T.Cubature Kalman Filtering for Continuous-discrete Systems:Theory and Simulations[J]. IEEE Transactions on Signal Processing,2010,58(10):4977-4993.

[10] Leong P H,Arulampalam S,Lamahewa T A,et al.A Gaussian-sum Based Cubature Kalman Filter for Bearings-only Tracking[J].IEEE Transactionson Aerospace andElectronicSystems,2013,49(2): 1161-1176.

[11] 傅惠民,吳云章,婁泰山.欠觀測條件下的增量Kalman濾波方法[J].機械強度,2012,34(1):43-47.

[12] 傅惠民,婁泰山,吳云章.欠觀測條件下的擴展增量Kalman濾波方法[J].航空動力學報,2012,27(4): 777-781.

[13] 傅惠民,婁泰山,吳云章.無跡增量濾波方法[J].航空動力學報,2012,27(7):1625-1629.

[14] 傅惠民,婁泰山,吳云章.增量粒子濾波方法[J].航空動力學報,2013,28(6):1201-1207.

編輯 顧逸斐

Incremental Cubature Kalman Filtering Under Poor Observation Condition

MA Li-li1,ZHAO Tian-tian1,CHEN Jin-guang1,2
(1.School of Computer Science,Xi’an Polytechnic University,Xi’an 710048,China;
2.School of Electronic Engineering,Xidian University,Xi’an 710071,China)

In some engineering applications,there are some cases like the effect of environment,the instability of measurement devices.Large error comes into being from the measurement equation in the nonlinear filtering system.The standard nonlinear filtering algorithm cannot remove this kind of system error.This paper addresses this problem,assumes that process noises and measurement noises are subject to Gaussian distribution,establishes the incremental measurement equation using the difference between the adjacent measurements,and obtains the cubature points and its weights using the third-degree spherical-radial rule.The cubature points are employed to approximate the integrals in the Bayesian filtering process numerically,and the incremental cubature Kalman filtering is obtained.Simulation results show that the filtering accuracy of the incremental cubature Kalman filtering is not only better than that of the standard cubature Kalman filtering but also better than that of the incremental extended Kalman filtering.The new algorithm can eliminate the system error of the measurement equation successfully.

incremental measurement equation;incremental cubature Kalman filtering;poor observation condition; Kalman filtering;state estimation;deep space exploration

1000-3428(2014)10-0228-04

A

TP391

10.3969/j.issn.1000-3428.2014.10.043

國家自然科學基金資助項目(61201118);中國博士后科學基金資助項目(2013M532020);陜西省教育廳科研計劃基金資助項目(14JK1304);國家級大學生創新創業計劃基金資助項目(201310709006)。

馬麗麗(1979-),女,講師,主研方向:信息融合,目標跟蹤;趙甜甜,本科生;陳金廣,副教授、博士。

2013-09-09

2013-10-31E-mail:xamll@163.com

中文引用格式:馬麗麗,趙甜甜,陳金廣.欠觀測條件下的增量容積卡爾曼濾波[J].計算機工程,2014,40(10):228-231,238.

英文引用格式:Ma Lili,Zhao Tiantian,Chen Jinguang.Incremental Cubature Kalman Filtering Under Poor Observation Condition[J].Computer Engineering,2014,40(10):228-231,238.