統(tǒng)計(jì)線性化無(wú)序量測(cè)更新算法

王 煒，李 丹，黃心漢

（1.海軍工程大學(xué)理學(xué)院，武漢 430033；2.武漢理工大學(xué)理學(xué)院，武漢 430070；3.華中科技大學(xué)，武漢 430074）

統(tǒng)計(jì)線性化無(wú)序量測(cè)更新算法

王 煒1，李 丹2，黃心漢3

（1.海軍工程大學(xué)理學(xué)院，武漢 430033；2.武漢理工大學(xué)理學(xué)院，武漢 430070；3.華中科技大學(xué)，武漢 430074）

在實(shí)際的多傳感器系統(tǒng)中，由于各種傳感器具有不同的采樣率，預(yù)處理時(shí)間，以及數(shù)據(jù)通信延遲，因而會(huì)出現(xiàn)多個(gè)傳感器量測(cè)不同步到達(dá)融合中心的現(xiàn)象。進(jìn)一步地，當(dāng)較早時(shí)刻產(chǎn)生的量測(cè)在較晚時(shí)刻產(chǎn)生的量測(cè)之后到達(dá)融合中心時(shí)，無(wú)序量測(cè)問(wèn)題就出現(xiàn)了。針對(duì)離散時(shí)間非線性系統(tǒng)，提出基于統(tǒng)計(jì)線性化固定點(diǎn)平滑器的最佳統(tǒng)計(jì)線性化無(wú)序量測(cè)算法，它可以處理單步和兩步延遲（甚至多步）無(wú)序量測(cè)，且能達(dá)到與重新濾波法相同的估計(jì)精度。

目標(biāo)跟蹤，濾波估計(jì)，無(wú)序量測(cè)

引言

在實(shí)際的多傳感器系統(tǒng)中，由于各種傳感器具有不同的采樣率，預(yù)處理時(shí)間以及數(shù)據(jù)通信延遲，因而會(huì)出現(xiàn)多個(gè)傳感器量測(cè)不同步到達(dá)融合中心的現(xiàn)象。當(dāng)較早時(shí)刻產(chǎn)生的量測(cè)在較晚時(shí)刻產(chǎn)生的量測(cè)之后到達(dá)融合中心時(shí)，無(wú)序量測(cè)（OOSM）問(wèn)題就出現(xiàn)了。按照問(wèn)題的復(fù)雜程度，OOSM問(wèn)題可分為單個(gè)OOSM更新和多個(gè)OOSM更新兩類(lèi)［1］。許多實(shí)際的多傳感器系統(tǒng)會(huì)出現(xiàn)OOSM問(wèn)題［2］，并且當(dāng)OOSM頻繁出現(xiàn)時(shí)會(huì)影響和惡化系統(tǒng)性能，導(dǎo)致系統(tǒng)不穩(wěn)定。傳統(tǒng)的濾波算法對(duì)OOSM更新問(wèn)題不適合，必須針對(duì)性地提出處理OOSM算法。目前，已有一些綜述文獻(xiàn)［3-5］對(duì)該問(wèn)題引起了充分的關(guān)注。

一般地，所謂的重新濾波法是不可用的，必須針對(duì)性地提出處理OOSM的特定算法［5］。針對(duì)離散時(shí)間非線性系統(tǒng)，已有的OOSM處理算法主要是EIFAsyn算法［6-7］，Exact-PF算法［8］，OOSM-PF算法［9］，存儲(chǔ)有效性粒子濾波（SEPF）算法［10］，無(wú)跡OOSM濾波算法［11］。另外，文獻(xiàn)［12］提出了OOSM平滑處理算法。算法EIFAsyn是在IFAsyn算法的基礎(chǔ)上開(kāi)發(fā)的。由于EIFAsyn算法是將EIF嵌入IFAsyn算法框架內(nèi)后形成的算法，因此，不可避免地帶有擴(kuò)展卡爾曼濾波器（EKF）和擴(kuò)展信息濾波器（EIF）所具有的一些缺點(diǎn)。當(dāng)雅可比或海賽矩陣不存在時(shí)，EIFAsyn算法便不能工作了。而且，對(duì)系統(tǒng)方程做泰勒級(jí)數(shù)展開(kāi)的誤差越大，濾波算法的誤差也會(huì)越大。Exact-PF算法［8］是基于貝葉斯公式提出的精確粒子濾波算法。Ortan等人［9］基于粒子濾波算法，并修正經(jīng)典貝葉斯濾波公式后，提出處理多步延遲OOSM算法。只要近似概率密度的粒子充分多，隨著延遲步數(shù)的增加，該算法的結(jié)果會(huì)很接近最優(yōu)，然而它需占用很大的存儲(chǔ)量。Orgumer［10］的SEPF算法僅存儲(chǔ)以往粒子集對(duì)應(yīng)的充分統(tǒng)計(jì)量，因而計(jì)算量和存儲(chǔ)量小，且該算法在運(yùn)行時(shí)沒(méi)有改變粒子集本身，而只改變它們的權(quán)值。但該算法對(duì)于含“信息量”充分大的OOSM會(huì)失效。Chen［11］基于Unscented變換提出估計(jì)精度至少可達(dá)二階的OOSM單步濾波近似算法。但是該算法是個(gè)近似算法（相對(duì)于順序UKF的濾波精度而言），而且只適用于單步延遲OOSM。

本文進(jìn)一步研究了離散時(shí)間非線性系統(tǒng)的單個(gè)OOSM更新問(wèn)題，提出一種基于統(tǒng)計(jì)線性化方法的單個(gè)無(wú)序量測(cè)更新算法。該算法在理論上是最優(yōu)的。若使用Unscented變換來(lái)運(yùn)算該算法中的期望值，那么該算法是一個(gè)與順序量測(cè)Unscented卡爾曼濾波（UKF）算法具有同樣精度的非線性系統(tǒng)濾波算法。

1 問(wèn)題的提出

考慮下面的離散時(shí)間非線性狀態(tài)空間模型：

其中，xk是k時(shí)刻目標(biāo)狀態(tài)，zk是k時(shí)刻傳感器量測(cè)，fk，k-1（·）是非線性狀態(tài)轉(zhuǎn)移函數(shù)，hk（·）是非線性量測(cè)函數(shù)，wk，k-1是從k-1時(shí)刻到k時(shí)刻的累積過(guò)程噪聲，vk是量測(cè)噪聲。

其中，時(shí)戳d滿(mǎn)足條件k-l＜d＜k-l+1。

本文中的濾波算法都基于下面的3個(gè)假設(shè)。

假設(shè)1：wk和vk是互不相關(guān)的高斯白噪聲，且具有如下統(tǒng)計(jì)特性

其中，Qk為非負(fù)定對(duì)稱(chēng)矩陣，Rk為對(duì)稱(chēng)正定矩陣。

假設(shè)2：初始狀態(tài)x0與wk，vk互不相關(guān)，且服從高斯分布，其均值和協(xié)方差矩陣為：

假設(shè)3：狀態(tài)xk與wk，vk互不相關(guān)，即：

2 離散時(shí)間非線性系統(tǒng)的單個(gè)無(wú)序量測(cè)算法

2.1 非線性狀態(tài)空間模型的統(tǒng)計(jì)線性化

使用統(tǒng)計(jì)線性回歸方法［13-14］，可將非線性狀態(tài)空間模型統(tǒng)計(jì)線性化。首先，令：

那么，非線性狀態(tài)方程（1）可統(tǒng)計(jì)線性化為：

其中，

其中，線性化誤差 efk，k-1服從均值為零，協(xié)方差為 Pefk，k-1的高斯分布，且與xk-1不相關(guān)。efk，k-1與wk-1不相關(guān)。

其次，令：

那么，非線性量測(cè)方程（2）可統(tǒng)計(jì)線性化為：

其中，線性化誤差ehk服從均值為零和協(xié)方差為Pehk的高斯分布，且與xk不相關(guān)。ehk與vk不相關(guān)。

2.2 統(tǒng)計(jì)線性化卡爾曼濾波器

假定在給定量測(cè)集Zk-1={z1，z2，…，zk-1}后，已計(jì)算出xk-1|k-1和Pk-1|k-1。將xk-1|k-1和Pk-1|k-1代入統(tǒng)計(jì)線性化狀態(tài)方程（3）后，可得卡爾曼時(shí)間更新方程為：

接著，給定量測(cè)zk，并將xk|k-1和Pk|k-1代入統(tǒng)計(jì)線性化量測(cè)方程（4）后，可得卡爾曼量測(cè)更新方程為：

2.3 統(tǒng)計(jì)線性化固定點(diǎn)平滑器

利用文獻(xiàn)［16］有關(guān)“固定點(diǎn)最優(yōu)平滑估計(jì)”的推導(dǎo)過(guò)程和思路，可得統(tǒng)計(jì)線性化固定點(diǎn)平滑公式如下：

其中，固定點(diǎn)為N，j=N+1，N+2，…。

2.4 基于統(tǒng)計(jì)線性化方法的單個(gè)無(wú)序量測(cè)算法

從計(jì)算量、存儲(chǔ)量和算法性能折中的角度來(lái)看，基于Unscented變換的OOSM算法是實(shí)際應(yīng)用中較不錯(cuò)的選擇。本文針對(duì)非線性狀態(tài)空間模型的統(tǒng)計(jì)線性化形式（3）和式（4），借助線性最小均方誤差（LMMSE）意義下的OOSM最優(yōu)更新公式［2］，并基于統(tǒng)計(jì)線性化固定點(diǎn)平滑器，提出一種單個(gè)無(wú)序量測(cè)處理算法。若使用Unscented變換來(lái)運(yùn)算該算法中的期望值，那么該算法應(yīng)當(dāng)是一個(gè)與順序量測(cè)Unscented卡爾曼濾波（UKF）算法具有同樣精度的非線性系統(tǒng)濾波算法。

2.4.1 算法的推導(dǎo)

下面應(yīng)用OOSM最優(yōu)更新公式［7］推導(dǎo)該算法。最優(yōu)OOSM更新公式為：

其中，zd為l步延遲OOSM。

實(shí)際上，基于統(tǒng)計(jì)線性化狀態(tài)空間模型（3）和模型（4），可知：

于是，

其中，

由式（5）可知

故得：

因?yàn)?/p>

故

又因?yàn)?/p>

故

若zd為單步延遲量測(cè)，那么：

其中，

這是因?yàn)?，由統(tǒng)計(jì)線性化狀態(tài)方程（3）可知efk，d與xd不相關(guān)。

從而，

其中，

若zd為兩步延遲預(yù)測(cè)量測(cè)：

其中，

類(lèi)似于式（11）的推導(dǎo)，可得：

其中，

從而，

在這里，本文只討論了單步和兩步延遲OOSM算法，原因是隨著OOSM延遲步數(shù)的增長(zhǎng)，OOSM對(duì)當(dāng)前狀態(tài)估計(jì)的影響會(huì)越來(lái)越小，所以不必考慮具有大延遲步數(shù)的OOSM。

2.4.2 計(jì)算公式

若zd為單步延遲量測(cè)，那么

其中，

若zd為兩步延遲量測(cè)，那么

其中，

將固定點(diǎn)平滑標(biāo)準(zhǔn)式（7）至式（10）應(yīng)用于OOSM算法，并令N=d（對(duì)應(yīng)傳感器收到OOSMzd的時(shí)刻td）和j=k。其中，時(shí)刻td的前后相鄰時(shí)刻為tk-l和tk-l+1。

通過(guò)下面的計(jì)算順序可計(jì)算得出xd|k和Pd|k

注意：上面的計(jì)算沒(méi)有用到OOSMzd，而只使用了不含zd的量測(cè)集Zk。

其中，

如果使用Unscented變換來(lái)計(jì)算其中的數(shù)學(xué)期望和協(xié)方差，那么該算法應(yīng)當(dāng)是一個(gè)與UKF順序?yàn)V波算法具有同樣精度的非線性系統(tǒng)濾波算法。

3 仿真試驗(yàn)

該仿真將對(duì)本文提出的統(tǒng)計(jì)線性化單個(gè)無(wú)序量測(cè)算法作出驗(yàn)證，且只針對(duì)離散時(shí)間線性狀態(tài)方程和非線性量測(cè)方程情形。并且，使用Unscented變換來(lái)計(jì)算該算法其中的數(shù)學(xué)期望和協(xié)方差。

仿真中考慮純方位目標(biāo)跟蹤問(wèn)題。假定在二維空間中，使用兩個(gè)同類(lèi)型同采樣率傳感器跟蹤一個(gè)運(yùn)動(dòng)目標(biāo)。假定傳感器s1和s2是靜止的，且兩傳感器分別位于坐標(biāo)（10 m，-10 m）和（10 m，10 m）處。真實(shí)目標(biāo)做勻速直線運(yùn)動(dòng)。目標(biāo)運(yùn)動(dòng)模型取作CV模型。真實(shí)目標(biāo)的初始運(yùn)動(dòng)狀態(tài)為x0=［0 m 0 m 1 m/s 0 m/s］T。

傳感器量測(cè)模型為：

仿真場(chǎng)景包含14個(gè)量測(cè)值，具有不同延遲的OOSM處在末尾。假設(shè)目標(biāo)初始狀態(tài)和協(xié)方差為：

假定每個(gè)傳感器的采樣周期為ΔTn，n-1=3 s，q為過(guò)程噪聲譜密度，σv為量測(cè)噪聲標(biāo)準(zhǔn)差。

考慮具有不同延遲步數(shù)的OOSMs，對(duì)應(yīng)的傳感器ID和測(cè)得的量測(cè)值的時(shí)戳，被列在表格1中。表1包含了3種不同的場(chǎng)景。表1中的每一行時(shí)戳的排列順序反映了量測(cè)到達(dá)處理中心的正確順序。這里，分別針對(duì)兩種算法完成了1 000次蒙特卡羅仿真，它們包括：使用UKF的量測(cè)重處理方法（重新濾波法）和使用本文提出的OOSM處理算法（該方法使用Unscented變換來(lái)計(jì)算期望值和協(xié)方差，且記該算法為UKF_RLS算法）。

表1 傳感器量測(cè)到達(dá)中心處理器的順序

本文計(jì)算了 t14=20.5 s時(shí)刻由重新濾波法（Optimal）［5］所得的狀態(tài)估計(jì)x14|14的位置和速度RMS誤差，和t14=20.5 s時(shí)刻由本文算法（UKF_RLS）得到的無(wú)序量測(cè)估計(jì)14|14，k的位置和速度RMS誤差，并顯示于表2中。從表2可以看出，UKF_RLS算法與重新濾波法具有同等的精確度。

表2 末狀態(tài)估計(jì)的位置和速度RMS誤差比較

本文還計(jì)算了t14=20.5 s時(shí)刻由重新濾波法得到的狀態(tài)估計(jì)x14|14相應(yīng)于真實(shí)狀態(tài)的估計(jì)誤差協(xié)方差矩陣的跡，和 t14=20.5 s時(shí)刻由本文算法（UKF_RLS）得到的無(wú)序量測(cè)估計(jì)x14|14，k相應(yīng)于真實(shí)狀態(tài)的估計(jì)誤差協(xié)方差矩陣的跡，并顯示于下頁(yè)表3中。從表3可以看出，UKF_RLS算法與重新濾波法具有同等的精確度。

表3 誤差協(xié)方差跡的比較

總之，從仿真結(jié)果可看出本文所提出的OOSM處理算法的估計(jì)結(jié)果與重新濾波法幾乎相同，其結(jié)果之間的差異應(yīng)當(dāng)是由數(shù)值計(jì)算誤差造成的。

UKF_RLS算法不需要求狀態(tài)轉(zhuǎn)移矩陣的逆矩陣；它也不需要像Zl算法［5］那樣，需要存儲(chǔ)量測(cè)集（實(shí)際中的很多目標(biāo)跟蹤系統(tǒng)只存儲(chǔ)了目標(biāo)狀態(tài)的充分統(tǒng)計(jì)量，而非量測(cè)集）。文獻(xiàn)［11］僅僅提出基于UKF的可處理單個(gè)單步無(wú)序量測(cè)的近似算法，而UKF_RLS算法對(duì)單步和兩步無(wú)序量測(cè)都是最優(yōu)的。另外，由于UKF的估計(jì)性能本身就優(yōu)于EKF，所以UKF_RLS算法要優(yōu)于基于EKF的無(wú)序量測(cè)算法。

4 結(jié) 論

本文進(jìn)一步研究了離散時(shí)間非線性系統(tǒng)的單個(gè)OOSM更新問(wèn)題。針對(duì)非線性狀態(tài)空間模型的統(tǒng)計(jì)線性化形式，借助線性最小均方誤差意義下的OOSM最優(yōu)更新公式，并基于統(tǒng)計(jì)線性化固定點(diǎn)平滑器，提出一種單個(gè)無(wú)序量測(cè)處理算法。若使用Unscented變換來(lái)運(yùn)算該算法中的期望值，那么該算法應(yīng)當(dāng)是一個(gè)與順序量測(cè)Unscented卡爾曼濾波算法具有同樣精度的非線性系統(tǒng)濾波算法。

［1］ Zhang S，Bar-shalom Y.Optimal Update with Multiple Out-of-Sequence Measurements with Arbitrary Arriving Order［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48（4）:3116-3132.

［2］Zhang S，Bar-shalom Y.Optimal Removal of Out-of-Se quence Measurements from Tracks［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48（1）: 604-619.

［3］Bahador K，Alaa K，F(xiàn)akhreddine O K，et al.Multisensor Data Fusion:A Review of the State-of-the-art［J］.Information Fusion，2013，14（1）:28-44.

［4］潘 泉，王增福，梁 彥，等.信息融合理論的基本方法與進(jìn)展（Ⅱ）［J］.控制理論與應(yīng)用，2012，29（10）: 1233-1244.

［5］王 煒，黃心漢，王 敏，等.無(wú)序量測(cè)濾波更新算法綜述［J］，控制與決策，2012，27（1）:1-7.

［6］Eva Besada P，Jose A，Lopez O.Multisensor Out of Sequence Data Fusion for Estimating the State of Discrete Control Systems［J］.IEEE Transactions on Automatic Control，2009，54（7）:1728-1732.

［7］ Eva Besada P，Jose A，Lopez O，et al.Localization of Non-Linearly Modeled Autonomous Mobile Robots Using Out-of-SequenceMeasurements［J］.Sensors（Basel），2012，12（3）:2487-2518.

［8］ Zhang S，Bar-shalom Y.Out-of-Sequence Measurement Processing for Particle Filter:Exact Bayesian Solution［J］. IEEE Transactions on Aerospace and Electronic Systems，2012，48（4）:2818-2831.

［9］Orton M，Marrs A.Particle Filters for Tracking with Out of Sequence Measurements［J］.IEEE Trans.Aerospace and Electronic Systems，2005，41（2）:673-702.

［10］ Umut Orguner and Fredrik Gustafsson.Storage Efficient Particle Filters for the Out of Sequence Measurement Problem［C］//Proc.of 11th International Conference on Information Fusion.，2008:1-8.

［11］Chen J G，Li J，Gao X B.Single-Step-Lag OOSM Algorithm Based on Unscented Transformation［J］.Science China:Information Science，2011，54:664-673.

［12］Govaers F，Koch W.A Generalized Solution to Smoothing and Out-of-Sequence Processing［C］//15th International Conference on Information Fusion，2012:2545-2552.

［13］Vercauteren T，Wang X D.Decentralized Sigma Point Information Filters for Target Tracking in Collaborative Sensor Networks［J］.IEEE Transactions on Signal Processing，2005，53（8）:2997-3009.

［14］Shin E H.Sigma Point Based Estimation［J］.Journal of Global Positioning Systems，2009，8（2）:188-196.

［15］Rauch H E，Tung F，Striebel C T.Maximum Likelihood Estimates of Linear Dynamic Systems［J］.AIAA Journal，1965，3（8）:1445-1450.

［16］史忠科.最優(yōu)估計(jì)的計(jì)算方法［M］.北京:科學(xué)出版社，2001.

［17］王小旭，趙 琳，潘 泉，等.基于最小均方誤差估計(jì)的噪聲相關(guān) UKF設(shè)計(jì)［J］.控制與決策，2010，25（9）: 1393-1398.

Out-of-Sequence-Measurement Algorithm Based on Statistically Linearized Method

WANG Wei1，LI Dan2，HUANG Xin-han3
（1.School of Science，Naval University of Engineering，Wuhan 430033，China；
2.School of Science，Wuhan University of Technology，Wuhan 430070，China；
3.Huazhong University of Science and Technology，Wuhan 430074，China）

In a practical multi-sensor system，various sensors have different sampling rates，pretreatment time，and data communication delay，thus resulting in the phenomenon that a lot of sensor measurements don't arrive at the fusion center synchronously.Further，when the measurements generated at an earlier moment arrive at the fusion center after the measurements generated at a later time，the OOSM issue occurs.For the discrete-time nonlinear systems，the optimal statistically linearized OOSM algorithm based on the statistically linearized fixed-point smoother which can deal with single-step delay OOSMs and double-step delay OOSMs and have the same accuracy with the refiltering algorithm.

filtering estimation，target tracking，out-of-sequence measurement

TP273

A

1002-0640（2014）09-0076-06

2013-06-15

2013-09-07

王 煒（1975- ），男，山西長(zhǎng)治人，博士，講師。研究方向：信息融合。