容積Rauch-Tung-Striebel平滑器

楊峻巍

(中國(guó)西南電子技術(shù)研究所，成都 610036)

1 引 言

在系統(tǒng)狀態(tài)估計(jì)理論中，根據(jù)狀態(tài)向量與觀測(cè)向量在時(shí)間上存在的不同對(duì)應(yīng)關(guān)系，人們往往將其歸納為三類估計(jì)問題，即狀態(tài)的預(yù)測(cè)、濾波和平滑[1]，其中最為經(jīng)典的狀態(tài)估計(jì)理論為1960年卡爾曼所提出的卡爾曼(Kalman)濾波理論，現(xiàn)已在各種工程中得到了廣泛應(yīng)用。然而標(biāo)準(zhǔn)Kalman理論也有它自身的不足，其要求系統(tǒng)有必須滿足以下3個(gè)約束條件，即系統(tǒng)模型精確已知、系統(tǒng)的過程噪聲和觀測(cè)噪聲為方差已知的白噪聲序列以及系統(tǒng)必須為線性隨機(jī)系統(tǒng)。針對(duì)系統(tǒng)模型的不確定性，人們提出了各種自適應(yīng)濾波算法，如強(qiáng)跟蹤濾波算法、魯棒濾波算法以及基于噪聲估計(jì)的Sage-Husa自適應(yīng)濾波算法等。

與此同時(shí)，由于在工程實(shí)踐中，實(shí)際系統(tǒng)總是存在不同程度的非線性，如飛機(jī)和艦船的慣導(dǎo)系統(tǒng)、火箭的制導(dǎo)與控制系統(tǒng)、目標(biāo)跟蹤以及衛(wèi)星軌道/姿態(tài)的估計(jì)系統(tǒng)等都屬于非線性濾波問題[2-5]。對(duì)于上述系統(tǒng)的狀態(tài)估計(jì)，若采用線性Kalman濾波對(duì)其進(jìn)行濾波估計(jì)，則必將導(dǎo)致濾波的發(fā)散。為此，一些學(xué)者提出了擴(kuò)展Kalman濾波(Extended Kalman Filter,EKF)，該濾波算法的基本思想是將非線性系統(tǒng)線性化，然后應(yīng)用Kalman濾波技術(shù)進(jìn)行狀態(tài)估計(jì)，其優(yōu)點(diǎn)是：計(jì)算量小，且能夠很好地滿足弱非線性系統(tǒng)的狀態(tài)估計(jì)；其不足是：需計(jì)算Jacobi矩陣，對(duì)于維數(shù)較高的系統(tǒng)其計(jì)算量較大，對(duì)于某些系統(tǒng)甚至無法求取Jacobi矩陣；此外，對(duì)于強(qiáng)非線性系統(tǒng)，該濾波算法收斂速度較慢，甚至發(fā)散。針對(duì)EKF的上述不足，一些學(xué)者基于近似非線性函數(shù)的概率密度分布比近似非線性函數(shù)更容易的思想，提出的Sigma點(diǎn)卡爾曼濾波算法，其中包括Julier和Uhlman提出的無跡卡爾曼濾波(Unscented Kalman Filter，UKF)以及Norgaard和Ito提出的中心差分卡爾曼濾波(Central Differernce Kalman Filter，CDKF)[6-8]。

隨著非線性濾波算法研究的不斷深入，2009年Simon Haykin提出了一種新的非線性濾波算法，即容積卡爾曼濾波(Cubature Kalman Filter，CKF)，相比于Sigma點(diǎn)卡爾曼濾波算法，該濾波算法不僅計(jì)算量相對(duì)較小，且對(duì)于維數(shù)較高的強(qiáng)非線性系統(tǒng)，其濾波性能更優(yōu)[9-11]。

目前針對(duì)非線性系統(tǒng)狀態(tài)估計(jì)的研究，更多的是傾向于濾波算法的研究，而對(duì)于狀態(tài)平滑的問題研究甚少。眾所周知，狀態(tài)平滑由于其利用了更多的觀測(cè)信息而使得其比濾波算法具有更精確的狀態(tài)估計(jì)效果，因此在現(xiàn)實(shí)工程實(shí)踐中得到了廣泛的應(yīng)用，如導(dǎo)彈發(fā)射初始速度的估計(jì)、衛(wèi)星軌道重構(gòu)以及雷達(dá)目標(biāo)跟蹤等等[12-14]。

為此，本文基于非線性系統(tǒng)最優(yōu)平滑算法建立了容積Rauch-Tung-Striebel平滑器。首先，詳細(xì)推導(dǎo)了基于概率密度分布形式的非線性系統(tǒng)最優(yōu)平滑算法，并給出了基于Rauch-Tung-Striebel理論的最優(yōu)平滑迭代算法；然后將其與容積卡爾曼濾波理論相結(jié)合建立了容積Rauch-Tung-Striebel平滑器；最后對(duì)其可行性及有效性進(jìn)行了仿真實(shí)例證明。

2 問題描述

不失一般性，考慮如下的非線性系統(tǒng)狀態(tài)空間模型：

(1)

其中：xk∈Rn，yk∈Rm分別為系統(tǒng)狀態(tài)向量與量測(cè)向量；wk-1，vk分別為系統(tǒng)噪聲和量測(cè)噪聲，它們是相互獨(dú)立的高斯白噪聲，且分別滿足wk-1～N(0,Qk)，vk-1～N(0,Rk)；初始狀態(tài)x0與wk-1，vk互不相關(guān)，且滿足x0～N(m0,P0)；fk-1(·)，hk(·)分別表示非線性系統(tǒng)的系統(tǒng)函數(shù)與量測(cè)函數(shù)。

基于貝葉斯估計(jì)理論框架下的非線性系統(tǒng)平滑器，其本質(zhì)目的就是利用量測(cè)值y1:T={y1,y2,…,yT}獲取平滑分布p(xk|y1:T)的最優(yōu)或次優(yōu)近似，且其分布近似滿足高斯分布，即

(2)

3 非線性系統(tǒng)狀態(tài)最優(yōu)平滑算法

3.1 非線性系統(tǒng)最優(yōu)濾波算法

在貝葉斯?fàn)顟B(tài)估計(jì)理論框架下，由式(1)表示的系統(tǒng)狀態(tài)空間模型，也可表示為如下的所示的概率密度分布形式，即

(3)

其中，p(xk|xk-1)表示系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度,p(yk|xk)表示量測(cè)值的似然概率密度。

基于上述系統(tǒng)概率密度模型，其最優(yōu)濾波迭代算法可表示為時(shí)間更新和量測(cè)更新兩個(gè)基本步驟。

(1)時(shí)間更新

(4)

(2)量測(cè)更新

p(xk|y1:k)=Cp(yk|xk)p(xk|y1:k-1)

(5)

其中，C為歸一化因子，其具體表達(dá)式如下所示：

(6)

3.2 非線性系統(tǒng)最優(yōu)平滑算法

根據(jù)文獻(xiàn)[12]可知，上述非線性系統(tǒng)狀態(tài)空間模型的最優(yōu)平滑算法，可寫為如下兩種形式，且兩者理論上相互等價(jià)。

形式1 兩濾波平滑算法

該平滑算法可表示為如下的概率密度分布函數(shù)：

p(xk|y1:T)∝p(xk|y1:k)p(yk+1:T|xk)

(7)

其中，右邊第一項(xiàng)可通過最優(yōu)濾波算法進(jìn)行實(shí)現(xiàn),第二項(xiàng)則可用一反向?yàn)V波算法來實(shí)現(xiàn)。

形式2 前向-后向平滑算法

該平滑算法可表示為如下的概率密度分布函數(shù)：

p(xk|y1:T)=p(xk|y1:k)×

(8)

其中，p(xk|y1:k)表示當(dāng)前時(shí)刻狀態(tài)的濾波概率密度分布,p(xk+1|y1:k)表示k+1時(shí)刻狀態(tài)的預(yù)測(cè)概率密度分布。

3.3 非線性系統(tǒng)RTS平滑算法

為了建立非線性系統(tǒng)RTS平滑算法，將式(8)分解為以下三個(gè)步驟：

步驟1：建立y1:k條件下xk與xk+1的聯(lián)合概率密度函數(shù)：

p(xk,xk+1|y1:k)=p(xk+1|xk)p(xk|y1:k)

(9)

其中，p(xk|y1:k)表示當(dāng)前時(shí)刻的濾波概率密度分布；

步驟2：計(jì)算xk+1和y1:k條件下xk的概率密度函數(shù)：

(10)

其中

(11)

由于系統(tǒng)狀態(tài)空間模型具有馬爾科夫特性，因此有p(xk|xk+1,y1:T)=p(xk|xk+1,y1:k)，則可得

(12)

步驟3：計(jì)算y1:T條件下xk與xk+1的聯(lián)合概率密度函數(shù)：

p(xk,xk+1|y1:T)=p(xk|xk+1,y1:T)p(xk+1|y1:T)

(13)

其中,p(xk+1|y1:T)表示下一時(shí)刻的平滑分布。因此在xk+1邊緣化條件分布下，xk的平滑概率密度函數(shù)可表示為

(14)

3.4 非線性系統(tǒng)RTS平滑算法概率密度函數(shù)近似

假設(shè)1 狀態(tài)濾波的概率密度函數(shù)服從高斯分布，即

p(xk|y1:k)≈N(xk|mk,Pk)

(15)

假設(shè)2k+1時(shí)刻狀態(tài)平滑的概率密度函數(shù)服從高斯分布，即

(16)

根據(jù)上述兩個(gè)基本假設(shè)對(duì)3.3小節(jié)推導(dǎo)的非線性系統(tǒng)最優(yōu)RTS平滑算法進(jìn)行概率密度函數(shù)近似。

(1)對(duì)式(9)中的xk、xk+1聯(lián)合分布進(jìn)行高斯近似，則可得

(17)

(18)

(2)根據(jù)高斯分布準(zhǔn)則及式(17)，則式(12)也近似服從高斯分布，即

(19)

其中：

(20)

(21)

(22)

(3)根據(jù)假設(shè)2，則式(13)近似服從高斯分布，即

(23)

其中：

(24)

(25)

則可得k時(shí)刻狀態(tài)平滑服從高斯分布，即

(26)

其中：

(27)

(28)

4 容積RTS平滑算法

4.1 CKF基本原理

CKF的基本思想是，基于高斯域貝葉斯?fàn)顟B(tài)估計(jì)理論框架，將非線性濾波歸結(jié)為“非線性函數(shù)×高斯概率密度”的積分問題，即

(29)

其中，x表示待估系統(tǒng)狀態(tài)向量，f(x)表示求積非線性函數(shù)，Rn表示積分區(qū)間。

針對(duì)形如式(29)的積分問題，CKF濾波算法采用3自由度Spherical-Radial求容積規(guī)則，采用2n個(gè)等權(quán)值的容積點(diǎn)來實(shí)現(xiàn)非線性逼近，即

(30)

(31)

4.2 CKF迭代算法

針對(duì)形如式(1)的系統(tǒng)狀態(tài)空間模型，利用4.1小節(jié)所述的CKF基本原理，即可推導(dǎo)出CKF的迭代濾波算法，其包括時(shí)間更新和量測(cè)更新兩個(gè)基本步驟。

(1)時(shí)間更新

(32)

(33)

(34)

(35)

(36)

(2)量測(cè)更新

(37)

(38)

Yi,k|k-1=H(Xi,k|k-1)

(39)

(40)

(41)

(42)

(43)

(44)

(45)

4.3 容積CTS平滑算法

將上述非線性系統(tǒng)RTS平滑算法與標(biāo)準(zhǔn)CKF濾波算法進(jìn)行有機(jī)結(jié)合，即可得容積RTS平滑算法的遞推迭代算法，其具體過程包括四個(gè)步驟。

(1)建立容積點(diǎn)

(46)

其中

(47)

(2)通過系統(tǒng)狀態(tài)模型傳播容積點(diǎn)

(48)

(3)計(jì)算系統(tǒng)狀態(tài)的預(yù)測(cè)值、預(yù)測(cè)協(xié)方差矩陣及互協(xié)方差矩陣

(49)

(50)

(4)通過系統(tǒng)狀態(tài)模型傳播容積點(diǎn)

(51)

(52)

(53)

5 仿真實(shí)例

為了驗(yàn)證容積Rauch-Tung-Striebel平滑器的有效性，下邊通過一典型非線性系統(tǒng)模型對(duì)其進(jìn)行仿真分析，該系統(tǒng)模型為純方位目標(biāo)跟蹤模型，其狀態(tài)方程為

(54)

(55)

其中q表示噪聲譜密度，這里假設(shè)q=0.1。

其量測(cè)模型為

(56)

圖1 運(yùn)動(dòng)目標(biāo)真實(shí)軌跡Fig.1 The real trajectory of the moving object

同時(shí)取狀態(tài)估計(jì)的初始值為

分別采用EKF、UKF、CKF及相應(yīng)的RTS平滑算法對(duì)其進(jìn)行仿真驗(yàn)證，圖2～5分別表示采用標(biāo)準(zhǔn)CKF濾波算法與RTS-CKS平滑算法對(duì)運(yùn)動(dòng)目標(biāo)4個(gè)運(yùn)動(dòng)狀態(tài)的實(shí)時(shí)估計(jì)，由仿真結(jié)果可以直觀地看出，RTS-CKS平滑算法對(duì)目標(biāo)狀態(tài)的估計(jì)精度明顯優(yōu)于標(biāo)準(zhǔn)CKF濾波算法，這充分驗(yàn)證了RTS-CKS平滑算法相對(duì)于標(biāo)準(zhǔn)的CKF濾波算法在提高狀態(tài)估計(jì)精度和解決非線性系統(tǒng)狀態(tài)平滑問題等方面的有效性和可行性。

圖3 運(yùn)動(dòng)目標(biāo)在y方向上的位置估計(jì)Fig.3 The y-direction position estimation of the moving object

圖4 運(yùn)動(dòng)目標(biāo)在x方向上的速度估計(jì)Fig.4 The x-direction velocity estimation of the moving object

圖5 運(yùn)動(dòng)目標(biāo)在y方向上的速度估計(jì)Fig.5 The y-direction position estimation of the moving object

圖6～8分別表示EKF、UKF、CKF及相應(yīng)的RTS平滑算法對(duì)運(yùn)動(dòng)目標(biāo)的軌跡跟蹤，不難看出，各種非線性濾波算法的RTS平滑算法對(duì)軌跡的跟蹤精度明顯優(yōu)于對(duì)應(yīng)的非線性濾波算法本身。

圖6 EKF及RTS-EKS運(yùn)動(dòng)目標(biāo)軌跡跟蹤Fig.6 The trajectory tracking of the moving object with EKF and RTS-EKS

圖7 UKF及RTS-UKS運(yùn)動(dòng)目標(biāo)軌跡跟蹤Fig.7 The trajectory tracking of the moving object with UKF and RTS-UKS

圖8 CKF及RTS-CKS運(yùn)動(dòng)目標(biāo)軌跡跟蹤Fig.8 The trajectory tracking of the moving object with CKF and RTS-CKS

各種濾波算法及相應(yīng)的平滑算法估計(jì)均方誤差如表1所示。

表1 各種濾波算法及相應(yīng)的平滑算法估計(jì)誤差Table 1 The estimation error of the filtering methods and corresponding smoothing algorithm

由表1的統(tǒng)計(jì)結(jié)果可以看出，UKF的目標(biāo)跟蹤精度優(yōu)于EKF ，而CKF的目標(biāo)跟蹤精度優(yōu)于UKF，且RTS-CKS的目標(biāo)軌跡跟蹤精度也是最佳的，這充分說明了RTS-CKS相比于其他兩種平滑算法在解決非線性狀態(tài)的平滑估計(jì)問題時(shí)的優(yōu)越性。

6 結(jié) 論

本文基于貝葉斯估計(jì)理論框架推導(dǎo)了非線性系統(tǒng)RTS平滑算法，并將其與容積卡爾曼濾波相結(jié)合，建立了容積Rauch-Tung-Striebel平滑器；相比于RTS-EKS，該平滑算法無需計(jì)算復(fù)雜的雅克比矩陣，且沒有進(jìn)行一階線性化，因此狀態(tài)估計(jì)精度較高；相比于RTS-UKS，該平滑算法無需預(yù)先設(shè)定一些相關(guān)參數(shù)，且在系統(tǒng)維數(shù)較高的條件下，估計(jì)精度稍高，此外采樣點(diǎn)也略有減少，因而其計(jì)算量也會(huì)相應(yīng)有所減小。仿真結(jié)果表明，所建立的RTS-CKS平滑算法為解決非線性系統(tǒng)的狀態(tài)平滑問題提供一種切實(shí)可行的方案，且相比于EKF、UKF等非線性濾波算法及其平滑算法在估計(jì)精度上具有一定的優(yōu)越性。為了提高其工程實(shí)用性，今后需對(duì)其計(jì)算復(fù)雜度開展進(jìn)一步的研究。

[1] 付夢(mèng)印,鄧志紅,張繼偉.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京:科學(xué)出版社,2009.

FU Meng-yin,DENG Zhi-hong,ZHANG Ji-wei.Kalman filtering theory and its application in the navigation system[M].Beijing:Science Press,2009.(in Chinese)

[2] R van der Merwe.Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D].Portland,OR,USA:OGI School of Science & Engineering at Oregon Health & Science University,2004.

[3] 魏喜慶,宋申民.基于容積卡爾曼濾波的衛(wèi)星姿態(tài)估計(jì)[J].宇航學(xué)報(bào),2013,34(2):394-200.

WEI Xi-qing,SONG Shen-min.Cubature Kalman Filter-Based Satellite Attitude Estimation[J].Journal of Astronautics,2013,34(2):394-200.(in Chinese)

[4] Pesonen H,Piche R.Cubature-based Kalman filters for positioning[C]//Proceedings of 2010 IEEE International workshop on Positioning,Navigation and Communication.Dresden:IEEE,2010:45-49.

[5] 鹿傳國(guó),馮新喜,張迪.基于改進(jìn)容積卡爾曼濾波的純方位目標(biāo)跟蹤[J].系統(tǒng)工程與電子技術(shù),2012,34(1):28-33.

LU Chuan-guo,FENG Xin-xi,ZHANG Di.Pure bearing tracking based on improved cubature kalman filter[J].Systems Engineering and Electronics,2012,34(1):28-33.(in Chinese)

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

[7] 宋勇,胡波,李在清.基于平方根無跡卡爾曼濾波的數(shù)字預(yù)失真算法[J].電訊技術(shù)，2011,51(11):20-24.

SONG Yong,HU Bo,LI Zai-qing.A Novel Digital Predistortion Algorithm Baesd on Square Root Unscented Kalman Filter[J].Telecommunication Engineering,2011,51(11):20-24.(in Chinese)

[8] 王小旭,潘泉,程詠梅,等.中心差分卡爾曼平滑器[J].控制理論與應(yīng)用,2012,29(3):361-367.

WANG Xiao-xu,PAN Quan,CHENG Yong-mei,et al.Central difference Kalman smoother [J].Control Theory & Applications,2012,29(3):361-367.(in Chinese)

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

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

[11] 孫楓,唐李軍.Cubature卡爾曼濾波與Unscented卡爾曼濾波估計(jì)精度比較[J].控制與決策,2013,28(2):304-308.

SUN Feng,TANG Li-jun.Estimation precision comparison of Cubature Kalman filter and Unscented Kalman filter[J].Control and Decision,2013,28(2):304-308.(in Chinese)

[12] Simo S.Unscented Rauch-Tung-Striebel Smoother [J].IEEE Transactions on Automatic Control,2008,53(3):845-848.

[13] Saifudin R,Keigo W,Shoichi M,et al.An Unscented Rauch-Tung-Striebel Smoother for a Bearing Only Tracking Problem[C]//Proceedings of 2010 IEEE International Conference on Control,Automation and Systems.Gyeonggi-do:IEEE,2010:1281-1286.

[14] Deisenroth M P,Turner R D,Huber M F,et al.Robust Filtering and Smoothing with Gaussian Processes[J].IEEE Transactions on Automatic Control,2012,57(7):1865-1871.