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

第６章無(wú)序量測(cè)條件下的濾波方法

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

6.1引言

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

6.2問(wèn)題描述

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

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是服從高斯分布的零均值過(guò)程噪聲，h是非線性量測(cè)函數(shù)，wk是服從高斯分布的零均值量測(cè)噪聲。過(guò)程噪聲和量測(cè)噪聲互不相關(guān)，其方差分別為E[vk，jvTk，j]=Qk，j，E[wkwTk]=Rk。

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

xk=Fk，dxd+vk，d

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

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

(6-4)

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

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

6.3單步滯后無(wú)序量測(cè)算法

6.3.1回溯狀態(tài)

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

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

因此，式(6-10)中過(guò)程噪聲的條件均值為(6-19)所以，從時(shí)刻d到時(shí)刻k的狀態(tài)回溯值為(6-20)式(6-19)對(duì)應(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具有無(wú)序量測(cè)狀態(tài)估計(jì)的最優(yōu)更新過(guò)程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6.4基于UT變換的單步滯后無(wú)序量測(cè)算法

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

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

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

UT變換過(guò)程可以描述為：對(duì)于均值為xk|k、方差為Pk|k的n維隨機(jī)變量x，可以通過(guò)2n+1個(gè)西格瑪點(diǎn)χ(i)及其權(quán)值ω(i)進(jìn)行近似，即^其中(P)i表示矩陣P的第i列。為了求解cov[xk，zd|Zk，zd]和cov[zd|Zk]，我們使用UT變換方法進(jìn)行計(jì)算。具體實(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)計(jì)算k時(shí)刻狀態(tài)估計(jì)值的西格瑪點(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)計(jì)算d時(shí)刻狀態(tài)估計(jì)值的西格瑪點(diǎn)集及其權(quán)值{χ(i)d|k，ω(i)d|k}2ni=0；

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

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

Step4：計(jì)算追溯量測(cè)值(6-76)

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

Step6：計(jì)算d時(shí)刻追溯量測(cè)值之間的協(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。此時(shí)，狀態(tài)估計(jì)對(duì)應(yīng)的協(xié)方差矩陣為^(6-79)因此，由式(6-77)、式(6-78)和k時(shí)刻狀態(tài)向量協(xié)方差Pk|k即可求得Pk|d。假設(shè)過(guò)程噪聲的向后預(yù)測(cè)為零，則式(6-30)可以簡(jiǎn)化為(6-80)另外，對(duì)式(6-34)進(jìn)一步近似得(6-81)

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

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

輸入：

輸出：^

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

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

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

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

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

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

ξk;y

ξk;x

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

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