《目標(biāo)跟蹤系統(tǒng)中的濾波方法》課件第6章

第６章無序量測條件下的濾波方法

6.1引言6.2問題描述6.3單步滯后無序量測算法6.4基于UT變換的單步滯后無序量測算法6.5仿真實(shí)驗(yàn)及結(jié)果分析6.6小結(jié)

6.1引言

在時間序列信息的實(shí)時處理中，傳感器對信號進(jìn)行量測，然后將量測數(shù)據(jù)傳輸?shù)教幚碇行?。而在傳輸過程中，由于傳輸延遲等原因使得來自同一目標(biāo)的較早時刻的量測在較晚的量測之后到達(dá)處理中心的情況時有發(fā)生，這就是所謂的滯后無序量測(Out-Of-SequenceMeasurements，OOSM)問題。如果各個傳感器的滯后間隔最多只有一步，則稱之為單步滯后無序量測(Single-step-lagOOSM)。處理滯后量測問題的最優(yōu)方法為，從滯后時刻起，對所有量測數(shù)據(jù)進(jìn)行估計。此時相當(dāng)于沒有滯后情況發(fā)生[1]。但是最優(yōu)方法存在的缺點(diǎn)是需要存儲從滯后發(fā)生時刻起的所有量測值，存儲量較大，而且在滯后數(shù)據(jù)到達(dá)后，需要重新進(jìn)行狀態(tài)估計，計算量也較大。為了解決上述問題，人們提出了一系列次優(yōu)算法，目的是利用滯后的量測數(shù)據(jù)對當(dāng)前時刻的目標(biāo)狀態(tài)進(jìn)行再更新，以獲得更精確的狀態(tài)估計及其誤差協(xié)方差矩陣，典型的如Bar-Shalom提出的A1、B1和C1算法等[1-3]。文獻(xiàn)[4，5]從理論上分析了Bar-Shalom提出的A1算法的最優(yōu)性，指出其最優(yōu)性與過程噪聲的離散化模型有關(guān)，提出了一種DDM(DirectDiscrete-timeModel)條件下的改進(jìn)算法和一種與過程噪聲離散化模型無關(guān)的最優(yōu)無序量測濾波算法，提高了濾波的精度。文獻(xiàn)[6]采用等效量測的方法將文獻(xiàn)[2]、[3]中的單步滯后OOSM更新算法擴(kuò)展到了多步滯后OOSM更新算法。文獻(xiàn)[7]提出了基于最佳線性無偏估計(BestLinearUnbiasedEstimation，BLUE)準(zhǔn)則的OOSM更新算法。文獻(xiàn)[8，9]在單步OOSM算法的基礎(chǔ)上，提出了用于解決多個傳感器單步滯后OOSM融合問題的方法。盡管上述算法在應(yīng)用中取得了良好的效果，然而這些算法都是將卡爾曼濾波作為基礎(chǔ)濾波算法，因此只能用于解決線性高斯系統(tǒng)的狀態(tài)估計問題。對于非線性系統(tǒng)，則需要在上述算法的基礎(chǔ)上應(yīng)用擴(kuò)展卡爾曼濾波(ExtendedKalmanFilter，EKF)算法思想對其進(jìn)行擴(kuò)展，從而獲得相應(yīng)的基于EKF的A1、B1和C1等更新算法(以下簡稱EKFA1、EKFB1、EKFC1)。然而有些非線性問題并不能夠用EKF算法求解，例如當(dāng)非線性方程的雅可比矩陣(或Hessian矩陣)不存在時，基于EKF的OOSM更新算法失效。再者，如果量測函數(shù)是高度非線性的，在利用泰勒級數(shù)展開式做近似的過程中，誤差過大，從而造成整個更新結(jié)果誤差較大。針對這些情況，文獻(xiàn)[10，11]提出了基于粒子濾波的OOSM算法，可以處理任意非線性非高斯系統(tǒng)的估計問題。文獻(xiàn)[12-14]將不同的粒子濾波改進(jìn)算法應(yīng)用于OOSM更新過程，在一定程度上改善了濾波的性能。然而，基于粒子濾波的OOSM算法一方面需要存儲濾波過程中所有粒子及其權(quán)值，另一方面大量粒子參與運(yùn)算需要極大的運(yùn)算量，再加上它本身就是處理滯后量測問題的次優(yōu)算法，這與研究量測滯后算法的動機(jī)互相矛盾。本章在經(jīng)典的A1算法的基礎(chǔ)上，推導(dǎo)了非線性系統(tǒng)的單步滯后OOSM更新算法，提出了基于UT變換(UnscentedTransformation)[15，16]的單步滯后OOSM算法。該算法適用于動態(tài)方程是線性而量測方程是非線性的非線性高斯系統(tǒng)，在處理過程中不需要求解非線性量測方程的雅可比矩陣(或Hessian矩陣)。由于UT變換屬于確定點(diǎn)采樣方法，采樣點(diǎn)數(shù)目很少，一般為狀態(tài)向量維數(shù)的2倍加1，因此本章算法計算量增加幅度不大，具有較好的實(shí)時性。

6.2問題描述

假定給定系統(tǒng)的狀態(tài)向量為x，量測向量為z，其線性狀態(tài)方程和非線性量測方程如下

xk=Fk，k－1xk－1+vk，k－1

(6-1)

zk=h(xk，k)+wk

(6-2)

其中，F(xiàn)k，k－1是狀態(tài)轉(zhuǎn)移矩陣，vk，k－1是服從高斯分布的零均值過程噪聲，h是非線性量測函數(shù)，wk是服從高斯分布的零均值量測噪聲。過程噪聲和量測噪聲互不相關(guān)，其方差分別為E[vk，jvTk，j]=Qk，j，E[wkwTk]=Rk。

由式(6-1)可得從時刻d到時刻k(d<k)的狀態(tài)轉(zhuǎn)移方程為

xk=Fk，dxd+vk，d

(6-3)上述公式可寫為負(fù)時間的更新形式(從時刻k到時刻d的狀態(tài)轉(zhuǎn)移)，即

xd=Fd，k(xk－vk，d)

(6-4)

其中，狀態(tài)轉(zhuǎn)移矩陣Fk，d=F－1d，k。

假定在時刻k，處理中心已經(jīng)通過計算獲得了k時刻的狀態(tài)向量的充分統(tǒng)計量(6-5)(6-6)其中，k時刻的累計量測值Zk={zi}ki=1。并假定此時先前時刻d(d<k)的量測zd到達(dá)處理中心，我們要解決的問題是如何利用量測zd直接更新k時刻的狀態(tài)估計值及其協(xié)方差，即要求解其中，Zd={Zk，zd}。如果k－l≤d<k－l+1，則是l步滯后OOSM。特別地，當(dāng)l=1時，稱之為單步滯后無序量測(Single-step-lagOOSM或1-step-lagOOSM)。單步滯后OOSM的情形如圖6.1所示，本章只討論單步滯后OOSM問題。圖6.1單步滯后OOSM示意圖

6.3單步滯后無序量測算法

6.3.1回溯狀態(tài)

Bar-Shalom提出了用于解決單步滯后OOSM的A1算法[2-3]。該算法適用于線性高斯系統(tǒng)。對于弱非線性高斯系統(tǒng)，通過求解非線性函數(shù)的雅可比矩陣或Hessian矩陣，即可以得到k時刻的一階(或二階)近似的線性狀態(tài)轉(zhuǎn)移矩陣Fk,k－1或者線性量測矩陣Hk。此時，就可以直接利用A1算法求解單步滯后OOSM問題，從而獲得EKFA1算法。

假定線性高斯系統(tǒng)的狀態(tài)轉(zhuǎn)移方程和量測方程如式(6-1)和式(6-2)所示，則k時刻非線性量測函數(shù)的雅可比矩陣為(6-7)根據(jù)式(6-4)，可得(6-8)此外，狀態(tài)回溯的協(xié)方差是在Zk的條件下最后兩項(xiàng)xk和vk，d的交叉協(xié)方差。首先定義(6-9)為了獲得yk|k，使用線性估計的基本方程，即有^(6-10)(6-11)和當(dāng)所有隨機(jī)變量是聯(lián)合高斯分布時，這些方程產(chǎn)生條件均值。除此之外，這些方程可以產(chǎn)生線性最小均值方差誤差(LinearMinimumMeanSquareError，LMMSE)估計。式(6-10)中的第一個協(xié)方差為(6-12)為了計算式(6-12)，下式可寫為(6-13)接下來，利用上式，有(6-14)由于上式中相互正交，根據(jù)假設(shè)E[vk，jvTk，j]=Qk，j，有(6-15)利用上述結(jié)果和式(6-11)表示協(xié)方差，根據(jù)矩陣的分塊，可以明確地寫出(6-16)式(6-10)其它相關(guān)項(xiàng)是(6-17)該式是新息的標(biāo)準(zhǔn)協(xié)方差，其中新息為(6-18)

因此，式(6-10)中過程噪聲的條件均值為(6-19)所以，從時刻d到時刻k的狀態(tài)回溯值為(6-20)式(6-19)對應(yīng)的協(xié)方差為(6-21)式(6-8)中狀態(tài)項(xiàng)和噪聲項(xiàng)的交叉協(xié)方差為(6-22)此外，由標(biāo)準(zhǔn)卡爾曼濾波，易得(6-23)結(jié)合上述結(jié)果，可以獲得式(6-20)所表達(dá)的回溯狀態(tài)向量的協(xié)方差為(6-24)值得注意的是，給定Zk，上述公式的條件協(xié)方差都是獨(dú)立的。6.3.2具有無序量測狀態(tài)估計的最優(yōu)更新過程

根據(jù)式(6-24)，d時刻的回溯量測值的協(xié)方差為(6-25)由量測方程知

，結(jié)合式(6-4)可得k時刻的狀態(tài)和量測之間的協(xié)方差矩陣為(6-26)因此，k時刻狀態(tài)估計xk|k的具有一步無序量測值zd的更新方程為^(6-27)其中，xd|k根據(jù)式(6-20)計算得到，增益的計算公式為^(6-28)更新后的狀態(tài)估計的協(xié)方差為(6-29)6.3.3A1算法

現(xiàn)將具有一步滯后量測的A1算法過程敘述如下：

從時刻k到時刻d的回溯狀態(tài)為(6-30)與回溯狀態(tài)向量相關(guān)的協(xié)方差為(6-31)(6-32)(6-33)回溯狀態(tài)的協(xié)方差為(6-34)回溯量測值的協(xié)方差為(6-35)

k時刻的狀態(tài)和量測值之間的協(xié)方差為(6-36)更新中所用的增益計算為(6-37)具有一步無序量測zd的最近狀態(tài)估計xk|k的更新方程為^(6-38)更新后的狀態(tài)估計協(xié)方差為(6-39)6.3.4次優(yōu)算法B1和C1

在下列的B1算法和C1算法中，將回溯噪聲假定為0，因此為次優(yōu)算法。其中B1算法過程如下。

從時刻d到時刻k的回溯狀態(tài)為(6-40)與回溯狀態(tài)向量相關(guān)的協(xié)方差計算如下(6-41)(6-42)(6-43)回溯狀態(tài)的協(xié)方差為(6-44)回溯量測值的協(xié)方差為(6-45)k時刻的狀態(tài)向量和量測向量之間的協(xié)方差計算如下(6-46)增益計算公式為(6-47)

具有一步無序量測zd的最近狀態(tài)估計xk|k的更新方程為^(6-48)(6-49)更新后的狀態(tài)估計協(xié)方差為算法B1和算法A1之間的差別是式(6-40)和式(6-41)，它們分別是式(6-30)和式(6-31)的近似和簡化。

下面給出次優(yōu)算法C1的過程，該算法同樣在假定回溯噪聲為0的基礎(chǔ)上，進(jìn)一步做了簡化。該算法僅僅使用了式(6-20)、式(6-24)和式(6-26)的第一項(xiàng)。從時刻d到時刻k的回溯狀態(tài)為(6-50)回溯狀態(tài)的協(xié)方差為(6-51)

回溯量測值的協(xié)方差計算為(6-52)

k時刻的狀態(tài)向量和量測向量之間的協(xié)方差計算如下(6-53)

增益矩陣計算公式為(6-54)具有一步無序量測zd的最近狀態(tài)估計xk|k的更新方程為^(6-55)

更新后的狀態(tài)估計協(xié)方差為(6-56)算法C和算法B的差別僅在于式(6-44)被替換為式(6-51)。6.3.5B1和C1算法的均方誤差

在兩種次優(yōu)算法計算各自協(xié)方差的過程中，由于都忽略了式(6-19)的計算，因而其最終的狀態(tài)估計是非精確的。兩種算法的估計都是有偏的，相應(yīng)的矩陣均方誤差同樣受到該有偏估計的影響，下面計算這種有偏性對矩陣均方誤差的影響大小。

注意到兩種算法都使用了下列更新形式(6-57)其中*表示B1算法或者C1算法。上述估計的誤差可用下式表達(dá)(6-58)其中(6-59)相應(yīng)地，矩陣均方誤差為(6-60)過程噪聲的矩陣均方誤差為(6-61)其中最后一項(xiàng)由式(6-19)給出，易知其依賴于k時刻的量測值。為了獲得數(shù)據(jù)相互獨(dú)立結(jié)果，式(6-61)替換為

(6-62)(6-63)得到最終結(jié)果

6.4基于UT變換的單步滯后無序量測算法

6.4.1用UT變換解決單步滯后OOSM

假定系統(tǒng)方程如式(6-1)和式(6-2)所示，依據(jù)線性最小均值方差(LinearMinimumMeanSquareError，LMMSE)準(zhǔn)則估計方程[17]，可得(6-64)式(6-65)(6-66)下面首先來推導(dǎo)E[zd|k|Zk]，即

(6-67)由于E[vk，d]=0，式(6-67)在最后一步推導(dǎo)做了近似。此時(6-67)化簡為(6-68)式(6-66)中有cov[xk，zd|Zk，zd]和cov[zd|k|Zk]需要求解。前者是k時刻的狀態(tài)向量和d時刻的量測值之間的協(xié)方差，后者是d時刻追溯量測值之間的協(xié)方差。在量測方程是非線性的情況下，這兩個量可以通過基于EKF的單步滯后OOSM算法予以解決，但是這需要計算非線性方程的雅可比矩陣(或Hessian矩陣)。此時，如果雅可比矩陣(或Hessian矩陣)不存在，則不能使用基于EKF的方法。再者，如果量測函數(shù)是高度非線性的，在利用泰勒展開式做近似的過程中，誤差過大，就會造成整個更新結(jié)果誤差較大。為此，本章提出采用UT變換的方法來求解式(6-66)中的協(xié)方差，從而解決了基于EKF的方法所帶來的問題。通常認(rèn)為近似一個概率分布比近似一個非線性函數(shù)或者非線性變換更容易。UT變換的思想就是通過一些選定的點(diǎn)并且給定這些點(diǎn)相應(yīng)的權(quán)值來近似一個概率密度函數(shù)的分布[15，16]。UT變換過程中利用隨機(jī)變量估計值及其方差產(chǎn)生相應(yīng)采樣點(diǎn)集，通常稱為西格瑪點(diǎn)集(Sigmapoints)。權(quán)值的確定與狀態(tài)向量的維數(shù)及其它一些參數(shù)的選擇有關(guān)，與西格瑪點(diǎn)本身及其方差無關(guān)。對每個西格瑪點(diǎn)實(shí)施非線性變換從而獲得相應(yīng)的變換采樣點(diǎn)，非線性變換的均值和方差就可以通過這些變換采樣點(diǎn)得到。UT變換的好處是不需要計算非線性函數(shù)的雅可比矩陣(或Hessian矩陣)，對于強(qiáng)非線性變換的近似性能仍然較好。

UT變換過程可以描述為：對于均值為xk|k、方差為Pk|k的n維隨機(jī)變量x，可以通過2n+1個西格瑪點(diǎn)χ(i)及其權(quán)值ω(i)進(jìn)行近似，即^其中(P)i表示矩陣P的第i列。為了求解cov[xk，zd|Zk，zd]和cov[zd|Zk]，我們使用UT變換方法進(jìn)行計算。具體實(shí)現(xiàn)步驟如下：

輸入：xk|k，Pk|k，xd|k，Pd|k；

輸出：cov[xk，zd|Zk，zd]，cov[zd|Zk];

Step1：將xk|k，Pk|k代入式(6-69)、式(6-71)和式(6-73)，并利用式(6-70)、式(6-72)和式(6-74)計算k時刻狀態(tài)估計值的西格瑪點(diǎn)集及其權(quán)值{χ(i)k|k，ω(i)k|k}2ni=0；

Step2：將xd|k，Pd|k代入式(6-69)、式(6-71)和式(6-73)，并利用式(6-70)、式(6-72)和式(6-74)計算d時刻狀態(tài)估計值的西格瑪點(diǎn)集及其權(quán)值{χ(i)d|k，ω(i)d|k}2ni=0；

Step3：對Step2所得西格瑪點(diǎn)集進(jìn)行非線性變換，即^^^^(6-75)其中，由西格瑪點(diǎn)集的求解過程易知，ω(i)d|k=ω(i)

k|k，i=0，…，2n；

Step4：計算追溯量測值(6-76)

Step5：計算k時刻的狀態(tài)向量和d時刻追溯量測值之間的協(xié)方差，即(6-77)

Step6：計算d時刻追溯量測值之間的協(xié)方差，即(6-78)將式(6-77)和式(6-78)代入式(6-66)，求得OOSM狀態(tài)更新增益Wk，d。再將式(6-65)、式(6-66)、式(6-68)代入式(6-64)，可求得OOSM狀態(tài)更新向量xk|d。此時，狀態(tài)估計對應(yīng)的協(xié)方差矩陣為^(6-79)因此，由式(6-77)、式(6-78)和k時刻狀態(tài)向量協(xié)方差Pk|k即可求得Pk|d。假設(shè)過程噪聲的向后預(yù)測為零，則式(6-30)可以簡化為(6-80)另外，對式(6-34)進(jìn)一步近似得(6-81)

6.4.2單步滯后OOSM多傳感器量測融合方法

在多傳感器集中式量測融合過程中，每個傳感器都可能出現(xiàn)單步滯后OOSM(為簡單起見，只考慮單步滯后OOSM情況)。當(dāng)出現(xiàn)單步滯后OOSM量測時，采用上述基于UT變換的單步滯后OOSM算法對當(dāng)前時刻的狀態(tài)估計及其協(xié)方差序貫更新，可以獲得更為精確的狀態(tài)估計及其協(xié)方差。另外，在多個傳感器量測融合過程中，還可能出現(xiàn)某些時刻沒有接收到量測值，不能使用當(dāng)前時刻的量測值對一步狀態(tài)預(yù)測及其協(xié)方差進(jìn)行更新，此時可將一步狀態(tài)預(yù)測及其協(xié)方差作為當(dāng)前時刻的狀態(tài)估計及其協(xié)方差。假設(shè)k時刻輸入?yún)?shù)包括k－1時刻的狀態(tài)估計及其協(xié)方差xk－1|k－1和Pk－1|k－1、k時刻的量測值{zik}mi=0，以及稍后到達(dá)的單步滯后量測{zid}ni=0，其中m+n≤M，M表示傳感器的個數(shù)，則具體融合過程如下：

輸入：

輸出：^

Step1：若m>0，則使用集中式序貫融合算法進(jìn)行融合，融合結(jié)果為xk|k，Pk|k；若m=0，則利用狀態(tài)轉(zhuǎn)移方程，計算狀態(tài)一步預(yù)測值及其協(xié)方差，并將其作為k時刻的狀態(tài)估計及其協(xié)方差，即^(6-83)

Step2：對于{zid}ni=0，若n=0，則不做處理；若n>0，則循環(huán)應(yīng)用基于UT變換的單步滯后OOSM算法對k時刻的狀態(tài)估計及其協(xié)方差進(jìn)行更新，并把更新后的結(jié)果作為第k步的狀態(tài)估計及其協(xié)方差，即(6-84)

6.5仿真實(shí)驗(yàn)及結(jié)果分析

6.5.1實(shí)驗(yàn)?zāi)Ｐ?/p>

假設(shè)目標(biāo)做如下運(yùn)動[18]：①1～49步，目標(biāo)沿著x方向以初始速度1向前運(yùn)動；②50～100步，以角速度pi/2/51/T完成90°的順時針轉(zhuǎn)彎動作；③101～199步，繼續(xù)向前運(yùn)動；④200～250步，以角速度pi/2/51/T完成90°的順時針轉(zhuǎn)彎動作；⑤251～350步，繼續(xù)向前運(yùn)動；⑥351～400步，以角速度pi/2/51/T完成90°的順時針轉(zhuǎn)彎動作；⑦401～500步，繼續(xù)向前運(yùn)動。其中，T=0.01。傳感器1的位置為(－1km，

－2km)，傳感器2的位置為(1km，1km)。目標(biāo)運(yùn)動軌跡以及傳感器位置如圖6.2所示。圖6.2目標(biāo)運(yùn)動軌跡狀態(tài)向量為xk=[ξk;x

ξk;y

ξk;x

ξk;y]T，其分量分別表示x和y方向上的位置和速度分量。狀態(tài)方程為··(6-85)其中，過程噪聲vk服從高斯分布，其協(xié)方差矩陣為(6-86)其中功率譜密度q=0.12。量測方程為(6-87)6.5.2仿真結(jié)果及分析

分別采用EKFA1、EKFB1、EKFC1算法和本章算法，利用滯后量測對當(dāng)前濾波結(jié)果進(jìn)行更新。MonteCarlo仿真1000次，計算每一步的均方根誤差(RMS)。圖6.3給出了x和y方向的位置誤差，圖6.4給出了x和y方向的速度誤差。由圖可見，有序量測(In-SequenceMeasurements)融合效果最好；丟棄滯后量測(Discarded)的策略效果最差；本章算法比EKFA1算法濾波精度高一些。其中，In-Sequence表示在兩個傳感器融合過程中，沒有OOSM情況發(fā)生；Discarded表示在兩個傳感器量測融合過程中，對于出現(xiàn)的滯后量測，直接丟棄。在狀態(tài)估計的最初一段時間，本章算法性能比EKFA1算法性能改善很多，經(jīng)過多步濾波之后，本章算法雖然仍然優(yōu)于EKFA1算法，但是二者的誤差非常接近。圖6.3位置分量的估計誤差采用MonteCarlo方法仿真1000次，對不同方法的平均均方根誤差(AverageRootMe