配套課件-目標(biāo)跟蹤系統(tǒng)中的濾波方法

第1章緒論1.1濾波方法在目標(biāo)跟蹤系統(tǒng)中的地位和作用1.2狀態(tài)估計和融合方法的研究進(jìn)展及現(xiàn)狀1.3目標(biāo)跟蹤濾波性能評價準(zhǔn)則1.4本書內(nèi)容安排1.1濾波方法在目標(biāo)跟蹤系統(tǒng)中的地位和作用

目標(biāo)跟蹤是人們運(yùn)用各種觀測和計算手段，實(shí)現(xiàn)主體對被關(guān)注運(yùn)動客體的狀態(tài)建模、估計、跟蹤的過程。隨著航空、航天、航海事業(yè)的不斷發(fā)展以及現(xiàn)代戰(zhàn)爭信息化、網(wǎng)絡(luò)化特征的日益凸顯，對海底、海面、陸地、空中和太空中目標(biāo)跟蹤技術(shù)的精確性和實(shí)時性的要求在不斷提高。毋庸置疑，該技術(shù)在國防安全領(lǐng)域中發(fā)揮著重要作用。此外，在民用領(lǐng)域，目標(biāo)跟蹤技術(shù)也得到廣泛應(yīng)用，例如空中交通管制、機(jī)器人、視頻監(jiān)控以及存在于制造業(yè)中的工件定位等。一般意義下的目標(biāo)跟蹤技術(shù)通常包括三個部分：數(shù)據(jù)關(guān)聯(lián)、狀態(tài)估計及融合、航跡管理[1]，如圖1.1所示。在多目標(biāo)多傳感器測量環(huán)境下，數(shù)據(jù)關(guān)聯(lián)的作用很重要，不正確的數(shù)據(jù)關(guān)聯(lián)直接導(dǎo)致跟蹤精度降低甚至丟失目標(biāo)。數(shù)據(jù)關(guān)聯(lián)從所關(guān)聯(lián)數(shù)據(jù)的類型的角度可做如下劃分：量測和量測關(guān)聯(lián)、量測和局部估計關(guān)聯(lián)以及航跡和航跡關(guān)聯(lián)。數(shù)據(jù)關(guān)聯(lián)的難度體現(xiàn)在傳感器存在漏檢、虛警以及在測量值比較密集情況下的關(guān)聯(lián)。Sittler最早提出數(shù)據(jù)關(guān)聯(lián)的概念，Singer和BarShalom對數(shù)據(jù)關(guān)聯(lián)理論的研究和發(fā)展做出過重大貢獻(xiàn)。最為重要的數(shù)據(jù)關(guān)聯(lián)方法包括最近鄰方法、概率數(shù)據(jù)關(guān)聯(lián)、聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)、多假設(shè)方法以及上述方法的改進(jìn)算法等。航跡管理包括航跡起始、航跡終止、航跡維持等。圖1.1目標(biāo)跟蹤系統(tǒng)

1.2狀態(tài)估計和融合方法的研究進(jìn)展及現(xiàn)狀

1.2.1信息融合技術(shù)

信息融合通俗地講，是將不同來源、不同模式、不同媒質(zhì)、不同時間、不同地點(diǎn)、不同表示形式的信息進(jìn)行綜合，最后得到對被感知對象的更精確描述。信息融合與數(shù)據(jù)融合雖屬兩個不同的概念，但兩者很類似，人們所指的信息融合一般是指數(shù)據(jù)的融合，因此本書對這兩個概念不加區(qū)分。由于多傳感器測量信息具有冗余性和互補(bǔ)性，因此采用有效的融合方法可以得到更加可靠和更加準(zhǔn)確的信息。冗余性可以增加系統(tǒng)的健壯性，如果傳感器網(wǎng)絡(luò)中有部分節(jié)點(diǎn)遭到破壞，則可以利用其它正常工作的傳感器所攜帶的相同信息進(jìn)行彌補(bǔ)?；パa(bǔ)性可以提高信息的準(zhǔn)確性，可以充分利用不同傳感器的測量特征，從而獲取被觀測目標(biāo)的更準(zhǔn)確、更全面的信息。根據(jù)美國國防聯(lián)合指導(dǎo)實(shí)驗室(theUSJointDirectorsofLaboratories，JDL)的定義，信息融合是組合數(shù)據(jù)的過程，該過程能夠進(jìn)一步優(yōu)化估計和預(yù)測[3]。也就是說，如果能夠有效地利用多源信息，則能夠在一定程度上獲取更加可靠、更加精確的估計和預(yù)測信息。為了說明信息融合的特征，我們給出一個典型的信息融合例子，如圖1.2所示。在目標(biāo)跟蹤過程中，使用雷達(dá)(Radar)、可見光或紅外(EO/IR)和電子支援設(shè)備(ESM)對目標(biāo)進(jìn)行觀測，然后進(jìn)行融合。可以看出，不同類型的測量設(shè)備在探測性能、運(yùn)動參數(shù)估計和識別能力方面是不完全相同的。也就是說，這些傳感器的性能具有互補(bǔ)性，所獲得的測量信息也相應(yīng)地具有互補(bǔ)性，使用有效的融合方法進(jìn)行融合，能夠獲得各項指標(biāo)都較好的總體性能。此外，以各種途徑獲得的數(shù)據(jù)之間存在冗余，當(dāng)一部分?jǐn)?shù)據(jù)被破壞或者丟失之后，可以通過其它傳感器數(shù)據(jù)對該部分?jǐn)?shù)據(jù)進(jìn)行恢復(fù)，因此提高了系統(tǒng)的健壯性。圖1.2信息融合舉例信息融合在軍事上涵蓋的范圍是比較廣泛的，從最底層的數(shù)據(jù)預(yù)處理到戰(zhàn)場中的指揮決策，全都包含在內(nèi)。JDL實(shí)驗室對信息融合進(jìn)行了分層，給出了各層的作用范圍框架，如圖1.3所示。下面對每一層的定義簡單加以說明。圖1.3信息融合分層模型[3]第0層——次目標(biāo)數(shù)據(jù)估計：在對有偏的信號或者像素層數(shù)據(jù)進(jìn)行數(shù)據(jù)關(guān)聯(lián)和特征化的基礎(chǔ)上，對信號(即目標(biāo)觀測向量)進(jìn)行估計和預(yù)測。

第1層——目標(biāo)估計：在有偏的量測和航跡關(guān)聯(lián)的基礎(chǔ)上，對實(shí)體狀態(tài)進(jìn)行估計和預(yù)測，包括連續(xù)狀態(tài)估計和離散狀態(tài)估計。

第2層——態(tài)勢估計：估計和預(yù)測實(shí)體之間的關(guān)系，包括軍力結(jié)構(gòu)和交叉軍力關(guān)系、通信和感知影響、自然條件等。

第3層——影響估計：戰(zhàn)爭各方的計劃或者對其估計(或預(yù)測)的行為對態(tài)勢影響的估計和預(yù)測，包括多個參戰(zhàn)方行動計劃的相互作用等。第4層——過程優(yōu)化：自適應(yīng)地獲取和處理數(shù)據(jù)，以便支持戰(zhàn)爭目的。該處理涉及到計劃和控制，不是估計。該層任務(wù)主要是根據(jù)各層結(jié)果進(jìn)行判斷，指定有利于我方的部署，并將任務(wù)分配給各種資源，最終達(dá)到目前態(tài)勢和預(yù)測態(tài)勢有利于我方的目的。

然而上述分層模型只是一個框架，不夠具體。趙宗貴在此基礎(chǔ)上結(jié)合軍事實(shí)例對該模型涵蓋的內(nèi)容進(jìn)行了細(xì)化，如圖1.4所示。

信息融合技術(shù)涉及的范圍盡管很廣泛，但是現(xiàn)有的研究成果主要集中在低層(也就是第0層和第1層)。這主要是因為高層需要具體的實(shí)例環(huán)境以及相關(guān)的軍事理論和實(shí)踐知識，大部分理論研究者缺乏此類專業(yè)知識，因此，就公開的文獻(xiàn)來看，針對高層信息融合的研究開展得相對較少[5]。如果按照J(rèn)DL分層理論，本書的研究范疇屬于第1層次。

JDL劃分的第1層次中一個重要的研究內(nèi)容就是運(yùn)動目標(biāo)跟蹤。目標(biāo)的精確定位和運(yùn)動估計在現(xiàn)代戰(zhàn)場上對決定戰(zhàn)場態(tài)勢具有重要作用。因此，近半個世紀(jì)以來，世界各國尤其是發(fā)達(dá)國家，對信息融合的基礎(chǔ)理論研究和工程實(shí)踐都非常重視。在這期間，出現(xiàn)了眾多理論研究成果和專著。圖1.4信息融合分層在軍事中的實(shí)例[4]最具有影響的專著有Shalom等人的文獻(xiàn)[1]和文獻(xiàn)[6]、Blackman等人的文獻(xiàn)[7]以及Hall的文獻(xiàn)[8]等。還有許多優(yōu)秀的相關(guān)專著極大地推進(jìn)了信息融合技術(shù)的不斷發(fā)展[9-10]。我國在上世紀(jì)80年代中期開始數(shù)字雷達(dá)處理技術(shù)的研究，也出現(xiàn)了多部信息融合領(lǐng)域的著作[11-18]。尤其是2009至2010年，涌現(xiàn)出多部信息融合和目標(biāo)跟蹤領(lǐng)域的專著[19-25]，這些新著作必將會為信息融合技術(shù)在我國科研人員中的普及發(fā)揮重要作用。

在信息融合領(lǐng)域做出過巨大貢獻(xiàn)的有如下幾位：BarShalomY.、SamuelS.Blackman、Farina和李曉榕(LiX.R.)等人，他們長期活躍在信息融合領(lǐng)域，不斷地推動并引導(dǎo)信息融合技術(shù)的發(fā)展。國內(nèi)也展開了一系列研究，主要研究機(jī)構(gòu)有上海交通大學(xué)、國防科技大學(xué)、杭州電子科技大學(xué)、海軍航空工程學(xué)院、西安交通大學(xué)、西北工業(yè)大學(xué)和西安電子科技大學(xué)等單位。信息融合領(lǐng)域的重要刊物包括《IEEETransactionsonAutomaticControl》、《IEEETransactionsonAerospaceandElectronicSystems》、《IEEETransactionsonSignalProcessing》和Elsevier出版集團(tuán)的《Automatica》等。信息融合國際會議(InternationalConferenceonInformationFusion)是該領(lǐng)域的學(xué)術(shù)盛會。1.2.2目標(biāo)跟蹤技術(shù)

使用狀態(tài)空間估計方法對目標(biāo)進(jìn)行跟蹤，首先要對目標(biāo)的運(yùn)動模型和傳感器的測量模型建模。測量模型根據(jù)傳感器的性質(zhì)容易確定，然而，目標(biāo)的運(yùn)動模型事先難以確定。為了簡化問題，人們對常見的理想狀態(tài)下的目標(biāo)運(yùn)動進(jìn)行建模[26]。簡單的目標(biāo)運(yùn)動模型包括勻速運(yùn)動、勻加速運(yùn)動、零均值一階馬爾可夫模型、均值自適應(yīng)加速模型、已知轉(zhuǎn)彎角速度的常速度轉(zhuǎn)彎運(yùn)動和未知轉(zhuǎn)彎角速度的常速度轉(zhuǎn)彎運(yùn)動等。在基于模型的目標(biāo)跟蹤過程中，對目標(biāo)運(yùn)動情況的建模十分重要，模型的好壞直接決定著跟蹤的效果。然而在實(shí)際應(yīng)用中，目標(biāo)的運(yùn)動模型往往是未知的，而且在現(xiàn)代戰(zhàn)場上，目標(biāo)在運(yùn)動過程中會做各種各樣的運(yùn)動模式變換，以擺脫對方的追蹤和打擊。如果僅僅使用已知的模型進(jìn)行狀態(tài)估計，效果就會很差，在這種情況下，人們對機(jī)動目標(biāo)的跟蹤產(chǎn)生了濃厚的興趣。所謂機(jī)動目標(biāo)，是指目標(biāo)的運(yùn)動模式不斷變換，跟蹤系統(tǒng)無法準(zhǔn)確知道當(dāng)前目標(biāo)的運(yùn)動模型。李曉榕及其合作者十分關(guān)注機(jī)動目標(biāo)跟蹤方法的研究和該領(lǐng)域的進(jìn)展，并且分主題做了相關(guān)的綜述。1.2.3狀態(tài)估計技術(shù)

在目標(biāo)跟蹤過程中，傳感器測量信息不僅包含所需信號，同時也包含隨機(jī)觀測噪聲和干擾信號。估計是指通過對一系列帶有觀測噪聲和干擾信號的實(shí)際觀測數(shù)據(jù)進(jìn)行處理，從中得到所需要的各種參量的估計值的過程。通常估計問題可以分為兩類：一類是系統(tǒng)的結(jié)構(gòu)參數(shù)部分或全部未知，有待確定；二是系統(tǒng)中的部分或全部狀態(tài)變量不能直接測得。這兩類問題通常稱為參數(shù)估計和狀態(tài)估計。兩者的區(qū)別在于：參數(shù)估計是不隨時間變化或只隨時間緩慢變化的隨機(jī)變量；狀態(tài)估計是隨時間變化的隨機(jī)過程。對于狀態(tài)估計的國外專著如文獻(xiàn)[41-43]，國內(nèi)也涌現(xiàn)了一系列著作可供參考[44-48]。根據(jù)狀態(tài)向量和觀測向量在時間上存在的不同對應(yīng)關(guān)系，狀態(tài)估計問題可以分為預(yù)測、濾波和平滑。預(yù)測是濾波的基礎(chǔ)，濾波是平滑的基礎(chǔ)，因此下面僅針對主要的濾波算法進(jìn)行討論。假設(shè)xk|j表示根據(jù)j時刻和j時刻以前的測量值對k時刻的狀態(tài)xk作出的估計，則按照k和j的不同對應(yīng)關(guān)系，狀態(tài)估計可作如下劃分：

(1)當(dāng)k=j時的估計過程稱為濾波，即依據(jù)過去直到現(xiàn)在的觀測值來估計現(xiàn)在的狀態(tài)；

(2)當(dāng)k>j時的估計過程稱為預(yù)測，即依據(jù)過去直到現(xiàn)在的觀測值來預(yù)測未來的狀態(tài)；

(3)當(dāng)k<j時的估計過程稱為平滑，即依據(jù)過去直到現(xiàn)在的觀測值來估計過去的歷史狀態(tài)。^1.2.4估計融合技術(shù)

如果同時有多個傳感器對目標(biāo)進(jìn)行測量，則需要用到融合技術(shù)。根據(jù)觀測節(jié)點(diǎn)是否具有狀態(tài)估計能力，可以將其分為集中式、分布式和混合式三種結(jié)構(gòu)[80，81]。集中式結(jié)構(gòu)如圖1.5(a)所示，N個傳感器對目標(biāo)進(jìn)行觀測之后獲得量測值，然后將量測值送入融合中心，由中心處理器對這些量測信息進(jìn)行統(tǒng)一處理。分布式結(jié)構(gòu)如圖1.5(b)所示，每個傳感器帶有可以進(jìn)行局部估計的處理器，當(dāng)傳感器觀測到目標(biāo)信息之后，局部傳感器直接進(jìn)行處理，將得到的局部估計結(jié)果送入融合中心后，采用相應(yīng)算法進(jìn)行統(tǒng)一處理。前一種結(jié)構(gòu)需要傳輸?shù)男畔⒘看?，對中心?jié)點(diǎn)的處理能力要求高，然而由于原始數(shù)據(jù)能夠得到統(tǒng)一處理，因此，其處理結(jié)果精度高。后一種結(jié)構(gòu)網(wǎng)絡(luò)傳輸負(fù)載小，對中心節(jié)點(diǎn)的處理能力要求低，但是其處理結(jié)果精度也低。混合結(jié)構(gòu)性能則介于上述兩種結(jié)構(gòu)之間。集中式結(jié)構(gòu)下的融合算法較為簡單，常見的有擴(kuò)維融合和序貫融合兩種方法。而分布式結(jié)構(gòu)下，人們已經(jīng)提出了多種融合算法。

凸組合航跡融合算法(ConvexCombinationtracktotrackFusion，CCF)[80]不考慮各傳感器局部估計誤差之間的相關(guān)性。當(dāng)局部航跡都是傳感器航跡并且不存在過程噪聲，并且各傳感器在初始時刻的估計誤差也不相關(guān)時，簡單凸組合算法是最優(yōu)的。該算法只對各個局部傳感器的估計及其協(xié)方差矩陣進(jìn)行處理，需要傳送到中心節(jié)點(diǎn)的信息量少，與其它航跡融合算法相比運(yùn)算量小。圖1.5融合結(jié)構(gòu)最優(yōu)航跡融合算法(OptimaltracktotrackFusion，OF)[82]考慮了局部估計誤差之間的相關(guān)性，對航跡的過程噪聲有較好的抑制性。該算法性能和集中式擴(kuò)維融合算法性能相同，所以常常被作為衡量分布式航跡融合算法性能的基準(zhǔn)，但是該算法融合過程中需要的數(shù)據(jù)相對較多。在過程噪聲較小且數(shù)據(jù)采樣率較高的條件下，可以使用一種稱之為小航跡(TrackletFusion，TF)[83-85]的航跡融合算法。簡單凸組合融合算法在忽略噪聲相關(guān)的條件下利用狀態(tài)估計的誤差協(xié)方差對局部估計進(jìn)行加權(quán)求和。BarShalomCampo算法盡管考慮了噪聲相關(guān)情況，但解決的并不徹底[86]。類似的還有最大后驗概率法[87]、協(xié)方差交叉法[88-93]、聯(lián)邦濾波器法[94-96]等。其中聯(lián)邦濾波器法較為突出，通過放大各個噪聲分量來近似消除噪聲之間的相關(guān)，從而獲得了一種近似方法。該方法被美國宇航局作為標(biāo)準(zhǔn)算法，應(yīng)用在航空航天領(lǐng)域的估計融合過程中。此外，李曉榕創(chuàng)建的最優(yōu)線性估計融合方法將集中式、分布式和混合式結(jié)構(gòu)統(tǒng)一到一個算法框架下，具有良好的應(yīng)用參考價值。多傳感器的融合結(jié)果理論上要優(yōu)于單個傳感器估計，估計精度提高的幅度不但與融合算法有關(guān)，而且與傳感器量測噪聲之間的相關(guān)性有關(guān)。文獻(xiàn)[97]討論了在不同測量噪聲相關(guān)性條件下的部分航跡融合算法的誤差性能。

1.3目標(biāo)跟蹤濾波性能評價準(zhǔn)則

在目標(biāo)跟蹤濾波過程中，往往需要對濾波的準(zhǔn)確程度進(jìn)行評估，以判斷所采用算法的優(yōu)劣。由于單目標(biāo)系統(tǒng)和多目標(biāo)系統(tǒng)的性能評估準(zhǔn)則存在較大差異，因此，本節(jié)分別給出常用的濾波性能評價準(zhǔn)則。

1.3.1單目標(biāo)跟蹤濾波性能評價準(zhǔn)則

1.位置分量的誤差適配百分比

位置分量的誤差適配百分比(PercentageFitError，PFE)是目標(biāo)位置估計和真實(shí)位置之差的范數(shù)與真實(shí)位置范數(shù)之比。當(dāng)目標(biāo)真實(shí)位置和估計位置重合時,該值為0;當(dāng)目標(biāo)的估計位置偏離真實(shí)位置時，該值增大。在對比各種算法性能時,PFE值最小的算法精度是最好的。誤差適配百分比定義如下:

在實(shí)際應(yīng)用中，狀態(tài)向量中的真實(shí)分量是未知的，此時可以計算量測軌跡和預(yù)測軌跡或者估計軌跡之間的范數(shù)來替代真實(shí)分量。(1-1)

2.位置分量的均方根誤差

位置均方根誤差(RootMeanSquareError，RMSE)是指目標(biāo)真實(shí)的和估計的x、y和z方向位置的均方根誤差。該值是一個標(biāo)量，如果目標(biāo)真實(shí)位置和估計位置重合，則其值為0；如果估計位置偏離真實(shí)位置，則其值增大。在比較各種濾波算法性能時，該值越小，表明算法濾波精度越高。位置均方根誤差定義如下：(1-2)

3.位置分量的和方根誤差

位置分量的和方根誤差(RootSumSquareError，RSSE)是指目標(biāo)真實(shí)的和估計的x、y和z方向位置的誤差平方之和的平方根。該值是離散獨(dú)立變量，可以用圖來表示不同時刻的誤差結(jié)果。這些結(jié)果是一個誤差序列，如果目標(biāo)估計位置和真實(shí)位置相同，則相應(yīng)時刻的和方根誤差值為0；如果估計位置偏離真實(shí)位置，則該值增大。在比較各種濾波算法性能時，該值達(dá)到最低值的時間越短，說明算法收斂得越快，性能越好。位置分量的和方根誤差定義如下：(1-3)采用類似的式子可以計算目標(biāo)速度和目標(biāo)加速度的和方根誤差。

4.帶有理論邊界的狀態(tài)誤差

具有理論邊界的狀態(tài)誤差計算式為，通過該準(zhǔn)則(表達(dá)式)可將狀態(tài)誤差(可以是位置、速度、加速度、Jerk甚至Surge分量)和其理論邊界畫在曲線圖上。理論邊界等于協(xié)方差對角線元素值平方根的2倍。如果整個計算到的誤差曲線的95%都處于該邊界內(nèi)，就意味著狀態(tài)估計的精度良好。若x方向位置的狀態(tài)誤差計算為，則對應(yīng)的理論邊界(上下界)為。

5.帶有理論邊界的新息序列

帶有理論邊界的新息序列(或稱量測殘差)計算式為，該式的理論邊界為。將新息序列及其理論邊界一起顯示到曲線圖中，其中S是從卡爾曼濾波器輸出的新息協(xié)方差矩陣。理論邊界的值等于新息協(xié)方差矩陣的對角線元素值的平方根的2倍。如果誤差都位于邊界之內(nèi)，就意味著跟蹤器性能良好。

6.歸一化估計誤差平方

歸一化估計誤差平方計算為

(1-4)

該表達(dá)式利用狀態(tài)向量的協(xié)方差矩陣對狀態(tài)誤差平方做歸一化。一般情況下，其均值等于狀態(tài)向量的維數(shù)。如果該值位于理論邊界之內(nèi)，則意味著濾波器表現(xiàn)良好。該值的理論邊界下界(LowerBound)算式為；該值的理論邊界上界(UpperBound)算式為。其中，自由度p為NxNMCS;Nx是狀態(tài)向量的元素個數(shù)，NMCS是蒙特卡羅(MonteCarlo)仿真次數(shù)，χ是卡方分布符號。

7.具有理論邊界的歸一化新息平方

歸一化新息平方計算式為vS－1vT，該值利用新息的協(xié)方差對新息平方進(jìn)行歸一化。一般來說，其均值等于量測向量的維數(shù)。如果該值位于理論邊界之內(nèi)，則意味著濾波器表現(xiàn)良好。該值的理論邊界下界算式為；該值的理論邊界上界算式為。其中，自由度p為NzNMCS；Nz是量測向量的元素個數(shù)。1.3.2多目標(biāo)跟蹤濾波性能評價準(zhǔn)則

1.圓丟失概率

圓丟失概率(CircularPositionErrorProbability，CPEP)[98]用于評價目標(biāo)跟蹤丟失率的情況。假設(shè)k時刻實(shí)際和估計的多目標(biāo)狀態(tài)集合為Xk和Xk，則圓丟失率定義為^(1-5)

2.Hausdorff距離

Hausdorff距離[99]是數(shù)學(xué)中常用的度量兩個集合之間距離的方法。其優(yōu)點(diǎn)在于能夠較好地反映估計結(jié)果的局部性，但是它對目標(biāo)個數(shù)的估計誤差不敏感。對于目標(biāo)的真實(shí)狀態(tài)集X和估計狀態(tài)集X，Hausdorff距離定義為^(1-6)

3.Wasserstein距離

Hoffman和Mahler建議采用統(tǒng)計學(xué)中的Wasserstein距離[99]來定量地度量集合X和X之間的距離。對于目標(biāo)真實(shí)狀態(tài)集X和估計狀態(tài)集X，Wasserstein距離的定義為^^(1-7)

4.最優(yōu)子模型分配距離

最優(yōu)子模型分配距離(OptimalSubpatternAssignment，OSPA)[100]是一種可用來衡量集合之間差異程度的誤差距離。該距離建立在Wasserstein距離的基礎(chǔ)上，對Wasserstein距離度量集合之間的差異有所改善。

集合X={x1，x2，…，xm}和集合X={x1，x2，…，xn}之間的距離定義為

如果n≥m，則^^^^(1-8)如果n≤m，則(1-9)此外，當(dāng)p→∞時，有(1-10)其他1.3.3時間復(fù)雜度評價準(zhǔn)則

算法的時間復(fù)雜度是指執(zhí)行算法所需要的計算工作量。為了比較同一個問題的多種算法的時間復(fù)雜度高低，可以采用算法在執(zhí)行過程中所需要的基本運(yùn)算的次數(shù)來衡量[101]?？紤]到算法執(zhí)行中的基本運(yùn)算次數(shù)與實(shí)際求解的問題的規(guī)模有關(guān)，因此，算法所執(zhí)行的基本運(yùn)算次數(shù)是問題規(guī)模的函數(shù)，即A(n)=f(n)，其中n為問題的規(guī)模。

即便在同一問題規(guī)模的情況下，算法的時間復(fù)雜度也不是一成不變的。若算法執(zhí)行所需要的基本運(yùn)算次數(shù)取決于某一特定輸入，則需要采用其它的方法進(jìn)行度量。此時，常用的度量方法有平均性態(tài)法和最壞情況法。平均性態(tài)法是指用各種輸入情況下所需要的基本運(yùn)算次數(shù)的加權(quán)平均作為該算法時間復(fù)雜度的評價標(biāo)準(zhǔn)。算法的平均性態(tài)定義為(1-11)其中：x表示算法的輸入；p(x)表示輸入為x的概率；t(x)表示算法在輸入為x時所需要的基本運(yùn)算次數(shù)；Dn表示當(dāng)問題規(guī)模為n時，算法所有可能的輸入集合。最壞情況法采用算法執(zhí)行需要的最大基本運(yùn)算次數(shù)來表示算法的時間復(fù)雜度。最壞情況復(fù)雜度定義為

，其中參數(shù)含義同式(1-11)。在實(shí)際應(yīng)用中，考慮到時間復(fù)雜度計算中基本運(yùn)算次數(shù)分析的困難，通常把計算機(jī)仿真過程中算法運(yùn)行的平均時間作為度量標(biāo)準(zhǔn)。此時，計算公式為，其中：Tistart表示算法進(jìn)行第i次仿真時的開始時間，Tiend表示算法進(jìn)行第i次仿真時的結(jié)束時間，M表示算法進(jìn)行蒙特卡羅仿真總次數(shù)。

1.4本書內(nèi)容安排

第1章介紹本書的研究背景和意義，論述了信息融合中的目標(biāo)跟蹤技術(shù)的研究進(jìn)展及現(xiàn)狀，對目標(biāo)跟蹤系統(tǒng)中的濾波算法做了簡要回顧。第2章討論卡爾曼濾波算法和基于卡爾曼濾波的非線性濾波算法。第3章討論粒子濾波算法。第4章對具有等式約束的濾波算法進(jìn)行研究，在對具有約束的濾波算法進(jìn)行回顧的基礎(chǔ)上，提出了兩種與約束有關(guān)的算法，即：等式狀態(tài)約束下的粒子濾波算法和基于UT變換的收縮迭代非線性約束濾波算法。第5章研究了無序量測條件下的濾波問題，提出了一種基于UT變換的單步滯后無序量測算法。第6章研究了變分貝葉斯自適應(yīng)卡爾曼濾波算法，提出了一種新的雙重迭代VB_AKF算法;此外，在上述算法的基礎(chǔ)上，結(jié)合擴(kuò)維集中式融合和序貫集中式融合方法，提出了兩種新的多傳感器數(shù)據(jù)融合算法，即：基于VB_AKF的擴(kuò)維集中式融合算法和基于VB_AKF的序貫集中式融合算法。第7章討論丟包條件下的濾波方法，對近年來提出的具有丟包條件下的濾波算法進(jìn)行討論，提出了一類非線性系統(tǒng)中具有丟包情況的濾波算法。第8章針對估計融合方法進(jìn)行研究，對多種RTS平滑算法進(jìn)行討論，在此基礎(chǔ)上，提出了一種基于分段RTS的凸組合估計融合算法。第9章研究了幾種非線性濾波算法在目標(biāo)跟蹤中的應(yīng)用。第10章是相關(guān)的數(shù)學(xué)預(yù)備知識。第2章卡爾曼濾波和非線性系統(tǒng)濾波方法2.2卡爾曼濾波算法2.3擴(kuò)展卡爾曼濾波算法2.4不敏卡爾曼濾波算法2.5積分卡爾曼濾波算法2.6容積卡爾曼濾波算法2.7傅立葉厄米特卡爾曼濾波算法2.8中心差分卡爾曼濾波算法2.9小結(jié)

2.2卡爾曼濾波算法

2.2.1狀態(tài)空間模型

對于離散狀態(tài)空間模型或者貝葉斯非線性濾波模型，一般都包含有下列條件概率分布函數(shù)(2-1)該模型假定符合馬爾科夫理論，因此具有以下兩個性質(zhì)：

(1)狀態(tài)向量的馬爾可夫性質(zhì)。狀態(tài)值序列{xk:k=0，1，2，…}構(gòu)成了一個馬爾科夫序列，如果狀態(tài)值是離散的，那么序列是一個馬爾科夫鏈。符合該性質(zhì)，意味著k時刻的狀態(tài)估計xk只與k－1時刻的狀態(tài)值有關(guān)，而與k－1時刻之前(k－2，k－3，…，1)的狀態(tài)值無關(guān)，即

(2-2)同樣，k－1時刻的狀態(tài)估計值也只與k時刻的狀態(tài)值有關(guān)，而與k時刻之后(k+2，k+3，…，T)的狀態(tài)都無關(guān)，即(2-3)

(2)量測向量的條件獨(dú)立性。當(dāng)前時刻的量測值yk與當(dāng)前時刻的狀態(tài)值xk有關(guān)，而與k時刻之前(k－1，k－2，…，1)的量測值和狀態(tài)值無關(guān)，即(2-4)2.2.2最優(yōu)濾波方程

最優(yōu)濾波就是在給定歷史量測值的情況下，計算出每一時刻狀態(tài)向量xk的邊緣后驗分布值p(xk|y1:k)。

定理2.1

計算預(yù)測分布p(xk|y1:k－1)及濾波分布p(xk|y1:k)的遞歸方程由下述貝葉斯濾波方程給出。

(1)初始化。遞歸是從先驗分布p(x0)開始的。

(2)預(yù)測。狀態(tài)向量xk在第k步的預(yù)測分布可以通過ChapmanKolmogoro方程獲取(2-5)

(3)更新。當(dāng)獲得第k步的量測值yk后，后驗分布可由貝葉斯規(guī)則求取(2-6)其中歸一化常數(shù)Zk為(2-7)證明：在給定y1:k－1的情況下，xk和xk－1的聯(lián)合分布值計算如下(2-8)考慮到系統(tǒng)的馬爾可夫特性，在上式推導(dǎo)過程中丟棄了歷史量測值y1:k－1。給定y1:k－1時，xk的邊緣分布可以通過對式(2-8)在xk－1處求積分來獲取，也就是使用ChapmanKolmogoro方程(2-9)如果xk－1是離散的，那么式(2-9)中的積分變?yōu)樵趚k－1處的累加和。在給定y1:k的條件下，xk的分布可以通過貝葉斯規(guī)則進(jìn)行計算(2-10)其中，歸一化常數(shù)項如式(2-7)所示。式(2-10)在推導(dǎo)過程中丟掉了歷史量測數(shù)據(jù)y1:k－1，這是因為，在給定xk條件下yk獨(dú)立于歷史量測信息y1:k－1。2.2.3卡爾曼濾波

卡爾曼濾波是一種離散狀態(tài)空間模型下的線性最小方差估計。系統(tǒng)的動態(tài)方程和量測方程都是線性高斯的，通常表示為(2-11)(2-12)其中，xk∈Rn表示狀態(tài)向量，yk∈Rm表示量測向量，qk－1~N(0，Qk－1)和rk－1~N(0，Rk)分別是服從高斯分布的過程噪聲序列和量測噪聲序列，矩陣Fk－1是動態(tài)模型的轉(zhuǎn)移矩陣，Hk是量測模型矩陣。假設(shè)先驗分布也符合高斯分布，即x0~N(x0|0，P0)，則可將上述模型以概率形式表示為(2-13)(2-14)線性濾波模型(2-11)、(2-12)的最優(yōu)濾波方程可以通過下列近似方法獲取，其結(jié)果都符合高斯分布，即(2-15)(2-16)(2-17)上述分布中的參數(shù)可以根據(jù)下面的卡爾曼濾波器的預(yù)測和更新步驟計算。預(yù)測步驟為(2-18)(2-19)更新步驟為(2-20)(2-21)(2-22)(2-23)(2-24)初始狀態(tài)向量值先驗分布為x0~N(x0|0，P0)，其中給出了狀態(tài)向量的初始均值和方差?？柭鼮V波方程可以通過如下步驟進(jìn)行推導(dǎo):

(1)給定y1:k－1時，xk和xk－1的聯(lián)合分布為(2-25)其中(2-26)(2-27)xk的邊緣分布為(2-28)(2-29)(2-30)其中

(2)yk和xk的聯(lián)合分布如下

(2-31)其中(2-32)(2-33)(3)xk的條件分布為(2-34)其中(2-35)(2-36)

2.3擴(kuò)展卡爾曼濾波算法

2.3.1泰勒級數(shù)展開

考慮將高斯隨機(jī)變量x轉(zhuǎn)換成另外一個隨機(jī)變量y(2-37)(2-38)隨機(jī)變量y的概率密度表示為(2-39)其中，|J(y)|是逆變換g－1(y)的雅可比矩陣的行列式。然而，由于g往往是非高斯的，該分布難于直接獲得。對y的分布做一階泰勒級數(shù)近似的方法如下。若令x=m+δx,其中δx~N(0，P)，那么就可得函數(shù)g的泰勒級數(shù)展開為(2-40)其中：Gx(m)是一個雅可比矩陣，其各個元素定義為(2-41)G(i)xx(m)是一個Hessian矩陣(2-42)ei=[0…010…0]T是一個單位矩陣，只有在位置i的值為1,其余位置的元素值都為0。也就是說，在坐標(biāo)軸i的方向上是單位向量。也可以取泰勒級數(shù)展開式的前兩項對非線性函數(shù)進(jìn)行近似，即(2-43)計算該式在x=m時的期望值為(2-44)于是協(xié)方差近似計算為(2-45)向量x和y之間的互協(xié)方差也很重要，它可以通過下面的增廣變換來獲得(2-46)(2-47)其均值和協(xié)方差為(2-48)對于擴(kuò)展卡爾曼濾波方程的推導(dǎo)，需要一個一般化的變換形式(2-49)(2-50)其中，q和x相互獨(dú)立，x和y聯(lián)合分布的定義如前所述。

(1)含加性噪聲的線性化近似。對x和隨機(jī)變換y=g(x)+q的聯(lián)合分布的線性化為(2-51)其中(2-52)(2-53)(2-54)

Gx(m)是函數(shù)g關(guān)于狀態(tài)向量x的雅可比矩陣，當(dāng)x=m時，矩陣元素根據(jù)下式計算(2-55)

(2)含非加性噪聲的線性化近似。如果濾波模型中的過程噪聲是非加性的，即有(2-56)其中x和q是不相關(guān)的隨機(jī)變量。均值和方差可以通過將方程中的增廣向量(x，q)替換為向量x來獲取。正如方程(2-56)所示的，其增廣變換的均值和方差給出如下(2-57)(2-58)上述近似值的計算可以通過下面的規(guī)則求取。

對于x和y=g(x，q)的聯(lián)合分布，其線性化近似為(2-59)其中(2-60)(2-61)(2-62)Gx(m)是一個函數(shù)g關(guān)于變量x的雅可比矩陣，當(dāng)x=m，q=0時，該矩陣元素為(2-63)相應(yīng)地，Gq(m)也是一個函數(shù)g關(guān)于噪聲變量q的雅可比矩陣，該矩陣元素為(2-64)

(3)含有加性噪聲的二階線性化近似。二階線性化近似形式為(2-65)其中(2-66)(2-67)(2-68)其中，Gx(m)是形如式(2-55)的雅可比矩陣，而G(i)xx(m)是當(dāng)x=m時的Hessian矩陣，即(2-69)ei是一個單位向量，只在位置i取值為1，其余位置取值均為0。2.3.2擴(kuò)展卡爾曼濾波

1.帶有加性噪聲的一階擴(kuò)展卡爾曼濾波過程

假定過程噪聲和量測噪聲都是加性噪聲，擴(kuò)展卡爾曼濾波系統(tǒng)模型如下

xk=f(xk－1)+qk－1

(2-70)

yk=h(xk)+rk

(2-71)

其中：x∈Rn是狀態(tài)向量，y∈Rm是量測向量，qk－1~N(0，

Qk－1)和rk~N(0，Rk)分別是高斯過程噪聲及高斯量測噪聲。f是動態(tài)模型函數(shù)，h是量測模型函數(shù)。需要指出的是，函數(shù)f，h有時也將離散的時間變量k作為參數(shù)。

預(yù)測步驟為(2-72)(2-73)更新步驟為(2-74)(2-75)(2-76)(2-77)(2-78)

2.帶有非加性噪聲的一階擴(kuò)展卡爾曼濾波

具有非加性噪聲的擴(kuò)展卡爾曼濾波系統(tǒng)模型為(2-79)(2-80)其中，qk－1~N(0，Qk－1)和rk~N(0，Rk)分別是過程噪聲和量測噪聲。該模型在形式上更具有一般性。預(yù)測步驟為(2-81)(2-82)更新步驟為(2-83)

(2-84)(2-85)(2-86)(2-87)其中，F(xiàn)x(m)，F(xiàn)q(m)，Hx(m)和Hr(m)都是雅可比矩陣，矩陣元素分別為(2-88)(2-89)

3.帶有加性噪聲的二階擴(kuò)展卡爾曼濾波

類似地，利用泰勒級數(shù)二階展開近似方法，可以獲得二階擴(kuò)展卡爾曼濾波。下面給出算法步驟。

預(yù)測步驟為(2-90)(2-91)

更新步驟為(2-92)(2-93)(2-94)(2-95)(2-96)其中，矩陣Fx(m)和Hx(m)已經(jīng)在式(2-88)及式(2-89)中給出。矩陣F(i)xx(m)和H(i)xx(m)是相應(yīng)的Hessian矩陣，矩陣元素為(2-97)(2-98)非加性的情況可以通過同樣的過程獲得，這里不再贅述。與其它非線性濾波相比，EKF的優(yōu)點(diǎn)就是在實(shí)現(xiàn)方面相對比較簡單，時間復(fù)雜度低。構(gòu)建非線性化系統(tǒng)的近似線性化是解決非線性系統(tǒng)濾波問題的常規(guī)做法，且易于理解和接受。其缺點(diǎn)是EKF要求非線性系統(tǒng)的量測模型函數(shù)和動態(tài)模型函數(shù)是可微分的。EKF雖然被廣泛用于解決非線性系統(tǒng)的狀態(tài)估計問題，但其濾波效果在很多復(fù)雜系統(tǒng)中并不能令人滿意。模型的線性化誤差往往會嚴(yán)重影響最終的濾波精度，甚至導(dǎo)致濾波發(fā)散。另外，在許多實(shí)際應(yīng)用中，模型的線性過程比較繁瑣，而且也不容易得到。這就迫使人們不斷尋求新的非線性濾波算法。

2.4不敏卡爾曼濾波算法

2.4.1不敏變換

不敏卡爾曼濾波是在不敏變換的基礎(chǔ)上發(fā)展起來的。不敏變換的基本思想是由Julier等首先提出的，是用于計算經(jīng)過非線性變換的隨機(jī)變量統(tǒng)計的一種新方法。該方法不需要對非線性狀態(tài)和量測模型進(jìn)行線性化，而是對狀態(tài)向量的概率密度函數(shù)進(jìn)行近似。近似化后的概率密度函數(shù)仍然是高斯的，但它表現(xiàn)為一系列選取好的西格瑪采樣點(diǎn)。

假設(shè)x為一個nx維隨機(jī)向量，

為一非線性函數(shù)，并且y=g(x)。x的均值和協(xié)方差分別為和P。計算UT變換的步驟可簡單敘述如下。

Step1：首先計算2nx+1個西格瑪采樣點(diǎn)χi和相應(yīng)權(quán)值ωi

(2-100)(2-101)其中，[]i表示矩陣的第i列，nx為狀態(tài)向量的維數(shù)，λ是一個尺度參數(shù)，定義如下

λ=α2(nx+κ)－nx

(2-101)

式中參數(shù)α，κ決定西格瑪點(diǎn)以均值為原點(diǎn)的分散程度。

Step2：每個西格瑪采樣點(diǎn)通過非線性函數(shù)傳播，得到

yi=g(χi)，i=0，…，2nx

(2-102)

Step3：y的估計值和協(xié)方差估計如下(2-104)(2-103)2.4.2不敏卡爾曼濾波

假設(shè)非線性系統(tǒng)中的狀態(tài)向量的初始均值和協(xié)方差分別為。

UKF算法中的初始狀態(tài)向量則是由原始狀態(tài)向量、過程噪聲以及量測噪聲三者組成的擴(kuò)維向量，其初始值和協(xié)方差定義如下(2-106)(2-105)其中Q和R分別是過程噪聲和量測噪聲的協(xié)方差。UKF算法過程如下。

Step1：用式(2-100)計算西格瑪點(diǎn)的權(quán)值，用下式計算西格瑪點(diǎn)(此處把所有西格瑪點(diǎn)集看做一個向量)(2-107)(2-108)

Step2：西格瑪點(diǎn)的一步預(yù)測Step3：狀態(tài)預(yù)測(2-109)(2-110)Step3：計算量測預(yù)測采樣點(diǎn)(2-111)

Step4：估計量測預(yù)測值(2-112)Step5：估計新息協(xié)方差矩陣(2-113)Step6：估計互協(xié)方差矩陣(2-114)Step7：計算增益矩陣(2-115)

Step8：狀態(tài)更新(2-116)Step9：協(xié)方差更新(2-117)

2.5積分卡爾曼濾波算法

2.5.1高斯厄米特積分準(zhǔn)則

考慮一個在區(qū)間(a，b)上的加權(quán)可積函數(shù)g(x)，其積分表示為(2-118)其中W(x)是加權(quán)函數(shù)。采用m點(diǎn)數(shù)值積分對式(2-118)進(jìn)行近似計算(2-119)式中ξi是積分點(diǎn)，ωi是相應(yīng)的權(quán)值。假設(shè)一個隨機(jī)變量x具有高斯密度N(x;0，1)，那么函數(shù)g(x)的期望可以近似為(2-120)其中積分點(diǎn)ξi和相應(yīng)權(quán)值ωi的計算運(yùn)用了正交多項式和三對角矩陣之間的關(guān)系。假定J是一個對稱三對角矩陣，具有0對角元素，J矩陣的其它元素值通過下式計算(2-121)此處m是積分點(diǎn)個數(shù)，通常取3或者5。積分點(diǎn)，其中εi是J的第i個特征值；相應(yīng)的權(quán)值ωi=(νi)21，其中(νi)1是J的第i個特征向量的第一個元素。對于服從高斯分布的隨機(jī)向量x，其高斯密度函數(shù)為P(x)=N(x;0，)，為nx×nx單位矩陣。由于向量x中各個元素互不相關(guān)，式(2-120)可以擴(kuò)展到多維積分公式(2-122)其中，2.5.2積分卡爾曼濾波

通過線性回歸點(diǎn)(即高斯厄米特積分點(diǎn))，可以確定高斯密度。假設(shè)狀態(tài)的后驗概率可以用高斯分布來近似，可利用高斯厄米特積分點(diǎn)線性回歸系統(tǒng)的狀態(tài)方程和觀測方程，得到系統(tǒng)的先驗和后驗概率估計。積分卡爾曼濾波算法步驟總結(jié)如下。

預(yù)測步驟為(2-123)(2-124)(2-125)(2-126)更新步驟為(2-127)(2-128)(2-129)(2-130)(2-131)(2-132)(2-133)(2-134)

2.6容積卡爾曼濾波算法

2.6.1球面徑向規(guī)則

高斯分布下的非線性濾波問題可以歸結(jié)為一個積分計算問題，其中被積函數(shù)都是非線性函數(shù)和高斯密度乘積的形式，定義如下積分形式(2-135)其中被積函數(shù)定義在笛卡爾坐標(biāo)系中。為了對該積分形式進(jìn)行數(shù)值化，采取如下策略，將式(2-135)轉(zhuǎn)換成更常用的球面徑向形式，并采用新的球面徑向規(guī)則進(jìn)行數(shù)值積分。

1.球面徑向變換

在球面徑向變換中，把笛卡爾向量x∈Rn轉(zhuǎn)換成徑向標(biāo)量r和方向向量y的乘積，即：令x=ry，且yTy=1，則可得xTx=r2，r∈[0，∞]。式(2-135)在球面徑向坐標(biāo)系中可以寫為(2-136)式中:Un是球形的表面，定義為Un={y∈Rn|yTy=1};σ(·)是Un中的表面量測或面積元素。在單位權(quán)重函數(shù)w(y)=1時，令(2-137)則式(2-137)變?yōu)閺较蚍e分(2-138)球面徑向積分利用后續(xù)將要介紹的球面容積規(guī)則和高斯積分規(guī)則分別進(jìn)行數(shù)值計算，為了便于介紹上述規(guī)則，首先引入如下符號和定義。

滿足如下兩個條件的容積規(guī)則稱為完全對稱的：①若x∈D則有y∈D，其中y由x通過置換或者符號變換得到；②若x∈D，y∈D，則w(x)=w(y)，即集合中關(guān)于圓點(diǎn)對稱的元素的權(quán)值相等。

例如，實(shí)數(shù)集R是一個一維全對稱集，若x∈R則必有－x∈R并且有w(x)=w(－x)。對于n維對稱空間中的點(diǎn)u=(u1，u2，…，ur，0，…，0)∈Rn，若ui≥ui+1>0，i=1，2，…，r－1，則點(diǎn)u為一個生成元。把全對稱集中的生成元u=(u1，u2，…，ur，0，…，0)簡記為[u1，u2，…，ur]。若生成元中的各分量互不相同，則生成元的全對稱集有個元素。例如，[1]∈R2表示二維空間的點(diǎn)集：，該點(diǎn)集的生成元為。我們用[u1，u2，…，ur]i表示全對稱集[u1，u2，…，ur]中的第i個點(diǎn)。

2.球面容積規(guī)則

根據(jù)不變量理論，假定一個三階球面容積規(guī)則的最簡單形式為(2-139)由[u]i經(jīng)過置換和符號變換得到的點(diǎn)集是不變量。對于單項式，若為奇數(shù)，則式(2-139)式可以準(zhǔn)確積分。對于所有三階以下的單項式，只需要考慮單項式處于2的情況。因此，對于單項式f(y)=1和f(y)=y21，需要找出未知參數(shù)u和w，使其滿足全對稱容積規(guī)則

(2-140)(2-141)當(dāng)f(y)=1時，當(dāng)f(y)=y21時，

3.徑向規(guī)則

接下來介紹如何利用高斯求積分法求解徑向積分。高斯積分法是已知計算一維積分最為有效的數(shù)值積分方法。m點(diǎn)的高斯積分可以很好地逼近2m－1階多項式，即(2-142)其中，w(x)是[a，b]上已知的非負(fù)權(quán)函數(shù)，未知積分點(diǎn){xi}和相應(yīng)的權(quán)值{wi}具有唯一的解。比較式(2-136)和式(2-142)，我們得到區(qū)間[0，∞]的權(quán)函數(shù)為w(x)=xn－1exp(－x2)。為了把這種積分轉(zhuǎn)變成熟悉的形式，做變量代換，令t=x2，則有(2-143)對于一階高斯規(guī)則，有f(t)=1，t;f(x)=1，x2。但是它對于奇數(shù)階多項式f(x)=x，x3并不適用。但若把徑向規(guī)則與球面規(guī)則相結(jié)合求解式(2-135)，則所有的奇數(shù)階多項式的積分為零。這是因為對于球面規(guī)則(如式(2-136))，所有奇數(shù)階多項式在對稱空間的積分為零，因此一階通用高斯-拉格朗日規(guī)則數(shù)值積分需要一個點(diǎn)和相應(yīng)的權(quán)值。因此有~(2-144)

4.球面徑向規(guī)則

為了將球面規(guī)則和徑向規(guī)則結(jié)合起來，并將球面徑向規(guī)則用于高斯加權(quán)積分，下面需要介紹兩個有用的定理。

定理2.2

利用mr點(diǎn)高斯求積分規(guī)則對徑向積分進(jìn)行數(shù)值化計算(2-145)利用ms點(diǎn)球面規(guī)則對球面積分進(jìn)行數(shù)值化計算(2-146)于是得到一個ms×mr點(diǎn)球面徑向積分規(guī)則(2-147)

證明：求容積法規(guī)則可以設(shè)計成適用單項式的子空間，考慮如下被積函數(shù)(2-148)其中{di}是正整數(shù)。因此，有如下積分(2-149)此時，假定上式中的被積函數(shù)是一個d階單項式，即，作球面徑向變換得(2-150)把上式分解成為一個徑向積分和一個球面積分(2-151)運(yùn)用數(shù)值規(guī)則，得到(2-152)證畢。對于被積函數(shù)是階數(shù)不超過d的任何單項式，上述定理均適用。定理2.3

令函數(shù)w1(x)和w2(x)分別為w1(x)=exp(－xTx)和w2(x)=N(x;μ，Σ)，對于滿足的平方根矩陣，可以得到(2-153)證明：考慮式(2-153)左側(cè)。因為Σ是一個正定矩陣，故做矩陣分解，令，可以得到(2-154)證畢。對三階的球面徑向規(guī)則，mr=1和ms=2n，共需要2n個容積點(diǎn)。根據(jù)定理2.2和定理2.3，可用三階球面徑向規(guī)則計算標(biāo)準(zhǔn)的高斯加權(quán)積分，如下式(2-155)其中(2-156)使用求容積點(diǎn){εi，wi}數(shù)值化計算積分，就得到了CKF算法。需要指出的是，上面的容積點(diǎn)定義在笛卡爾坐標(biāo)系。2.6.2容積卡爾曼濾波

應(yīng)用統(tǒng)計線性回歸方法，可得容積卡爾曼濾波算法的狀態(tài)更新步驟和量測更新步驟如下[12，13]。

預(yù)測步驟為：

Step1：采用Cholesky或者奇異值方法分解協(xié)方差(2-157)Step2：計算容積點(diǎn)(i=1，2，…，m)(2-158)其中m=2nx。

Step3：計算預(yù)測狀態(tài)容積點(diǎn)(2-159)

Step4：估計預(yù)測狀態(tài)(2-160)Step5：估計預(yù)測協(xié)方差(2-161)更新步驟為：Step1：預(yù)測協(xié)方差矩陣進(jìn)行分解(2-162)Step2：計算容積點(diǎn)(i=1，2，…，m)(2-163)Step3：計算預(yù)測量測容積點(diǎn)(2-164)

Step4：估計預(yù)測量測(2-165)Step5：估計新息協(xié)方差矩陣(2-166)Step6：估計互協(xié)方差矩陣(2-167)Step7：計算卡爾曼濾波增益(2-168)Step8：估計狀態(tài)更新(2-169)

tep9：估計狀態(tài)協(xié)方差(2-170)2.7傅立葉厄米特卡爾曼濾波算法

2.7.1傅立葉厄米特級數(shù)展開

本節(jié)介紹傅立葉厄米特級數(shù)如何應(yīng)用于高斯隨機(jī)變量的非線性變換，以及如何看待它作為統(tǒng)計線性化的一般形式。

對于隨機(jī)變量x~N(m，P)，考慮統(tǒng)計線性化狀態(tài)模型

y=g(x)

(2-171)

假設(shè)其近似化形式為

g(x)≈b+Aδx

(2-172)

通過最小化表達(dá)式E[‖g(x)－b－Aδx‖2]獲得最優(yōu)矩陣A和向量b，其中δx=x－m。我們試圖將其一般化，使得它能進(jìn)行線性化近似。使用一個p階多項式進(jìn)行近似

(2-173)通過擴(kuò)展上述表達(dá)式并且令其導(dǎo)數(shù)為零，就可以確定最優(yōu)多項式的系數(shù)。該方法對于高階多項式的求解就會變得復(fù)雜。所幸的是，依據(jù)希爾伯特空間理論可以讓多項式的求解變得簡單。通過給標(biāo)量函數(shù)g和f定義內(nèi)積(2-174)其中x~N(m，P)。通過定義下列范數(shù)確定一個希爾伯特空間函數(shù)(2-175)希爾伯特空間理論表明，存在一個與希爾伯特空間對應(yīng)的正交多項式基。結(jié)果證明這些多項式基函數(shù)是尺度多元厄米特多項式，定義為(2-176)其中L是一個滿足P=LLT的矩陣，且有(2-177)這樣，就可以將一個滿足條件〈gi，gi〉<∞的任意向量函數(shù)g(x)擴(kuò)展為一個傅立葉厄米特級數(shù)，形式為(2-178)需要指出的是，因為H[0，…，0](x;m，P)=1，級數(shù)中的零階項恰好就是函數(shù)的期望E[g(x)]。利用基函數(shù)的正交性可以得到外積期望之和(2-179)在不考慮零階級數(shù)的情況下，我們可以得到g(x)協(xié)方差的表達(dá)式(2-180)由希爾伯特空間理論，可知對函數(shù)g(x)關(guān)于||·||2的最優(yōu)p階多項式近似，也即對式(2-173)的多項式展開，可以通過p階厄米特多項式的正交投影得到。那么通過對級數(shù)(式(2-178))進(jìn)行p階級數(shù)截斷就可以得到最優(yōu)p階多項式近似?，F(xiàn)在考慮使用數(shù)值計算的方法來求取級數(shù)的系數(shù)，然后選取其中的零階項作為均值，并通過上述對級數(shù)截斷的方法計算協(xié)方差的近似值。然而，用這種方法計算協(xié)方差并不是一個好的選擇，因為使用同樣的數(shù)值方法我們可以直接獲取協(xié)方差的值。實(shí)際上，依據(jù)下述結(jié)果，可以采用一種替代的方法獲取傅立葉厄米特級數(shù)系數(shù)，即

(2-181)上述公式是統(tǒng)計線性化方法導(dǎo)數(shù)的一般形式。由此，傅立葉厄米特級數(shù)就可以用下面的形式表達(dá)(2-182)且其協(xié)方差為(2-183)結(jié)果證明我們甚至不需要計算系統(tǒng)導(dǎo)數(shù)的期望值，因為可以使用以下方法求解。假定對于任意的m，P，使用下列形式來計算期望值

(2-184)接下來，有(2-185)為了獲得擴(kuò)維函數(shù)g(x)=(x，g(x))的聯(lián)合均值和協(xié)方差，同樣使用上述結(jié)果，并計算~(2-186)其中。從而得到下面的具有加性變換的傅立葉厄米特級數(shù)近似規(guī)則。對于x~N(m，P)，q~N(0，Q)，隨機(jī)變量x和非線性變換y=g(x)+q的聯(lián)合分布的傅立葉厄米特級數(shù)為(2-187)其中，，(2-188)且有如下定義(2-189)(2-190)矩陣G定義如下^(2-191)當(dāng)級數(shù)在截斷時，上述近似形式的均值及互協(xié)方差總是很精確的，協(xié)方差也可以精確到p階多項式。一階近似等同于統(tǒng)計線性化近似方法。用與上述類似的方法也可以獲得非加性變換的傅立葉厄米特近似過程。2.7.2傅立葉厄米特卡爾曼濾波

通過傅立葉厄米特級數(shù)展開，我們得到了傅立葉厄米特卡爾曼濾波算法[14]。該算法是統(tǒng)計線性化濾波的一般化形式。為了實(shí)現(xiàn)這種濾波算法，需要下述表達(dá)式的近似形式(2-192)(2-193)下面的式子同樣需要近似形式(2-194)(2-195)帶有加性噪聲的傅立葉厄米特卡爾曼濾波的狀態(tài)預(yù)測步驟和更新步驟如下:

預(yù)測步驟為(2-197)更新步驟為(2-198)

(2-199)(2-200)(2-201)(2-202)其中矩陣H

的元素定義為^(2-203)

2.8中心差分卡爾曼濾波算法

2.8.1Stirling插值公式

Stirling插值可以解決任意函數(shù)的多項近似問題。首先考慮單變量函數(shù)情況，然后再將其擴(kuò)展到多維情況。如果函數(shù)f是解析函數(shù)，將其圍繞著x=x做泰勒級數(shù)展開－(2-204)一種最常用的近似方法就是對上述級數(shù)進(jìn)行有限項的截斷。保留的項數(shù)越多，得到的近似也就越精確。類似地，可以通過插值公式來獲取多項式的近似形式。這些插值公式通常不要求對函數(shù)求導(dǎo)，而是對有限個函數(shù)進(jìn)行計算。因此，該方法可能會更容易獲取非線性函數(shù)的線性近似形式。下面考慮一種稱為Stirling的差值公式。首先定義兩個操作符δ，μ如下(2-205)其中h是選定的區(qū)間長度。在x=x處，采用Stirling插值公式進(jìn)行多項式逼近，得到－(2-206)(2-207)通常－1<p<1。考慮到我們要著重介紹一階和二階多項式逼近，因此將式(2-207)簡化為(2-208)其中(2-209)(2-210)式(2-208)可以解釋為把求導(dǎo)項替換為中心差分項的泰勒近似。為了評估多項式的逼近程度，用展開的泰勒級數(shù)代替和，得到(2-211)可以看出式(2-211)右邊的前3項和區(qū)間長度h無關(guān)。

現(xiàn)在考慮多維的情況。設(shè)x為一個向量，x∈Rn，y=f(x)，有多種不同的方法可以將插值公式擴(kuò)展成為多維的，但是在這之前，我們首先想到多維泰勒級數(shù)展開形式。當(dāng)x=x時，函數(shù)f的泰勒級數(shù)展開為－(2-212)其中，算子DiΔxf表示為(2-213)上述算子同樣可以表示為

(2-214)那么，采用多維插值公式對函數(shù)進(jìn)行二階多項式截斷可得(2-215)其中，差分算子DΔxf和D2Δxf可分別表示為~~(2-216)(2-217)式中δp是局部插分算子，且有(2-218)式中ep表示第p個元素為1的單位向量。均值運(yùn)算符μ可以做類似擴(kuò)展。式(2-215)僅僅是一個插值公式向多維擴(kuò)展的例子，為了說明如何推導(dǎo)其它形式的擴(kuò)展，首先介紹對x的線性化變換

z=S－1x

(2-219)函數(shù)f定義如下~(2-220)函數(shù)f

和f有著相同的泰勒級數(shù)逼近形式。而采用式(2-215)多項式插值公式，f，f顯然得不到相同的結(jié)果。這是因為~~(2-221)2.8.2中心差分逼近

隨機(jī)向量x的均值和協(xié)方差分別定義為(2-222)(2-223)然后再定義(2-224)(2-225)由于f是非線性函數(shù)，不可能計算出其精確的均值，通常情況下采用一階或二階多項式逼近形式代替。此處，我們用插值公式(2-215)來獲取f的多項式近似形式。首先把協(xié)方差矩陣的Cholesky分解因子作為變換矩陣(2-226)該變換有時也是對x進(jìn)行隨機(jī)解耦，使得z的各個元素之間相互無關(guān)，即(2-227)

1.一階近似

首先對函數(shù)使用一階截斷插值公式進(jìn)行逼近(2-228)根據(jù)定義有E[Δz]=0，則式(2-228)的期望為(2-229)如前所述，由于Δz均值為0，因此一階項可以忽略。此外，有E[ΔziΔzTj]=0，i≠j。一階估計協(xié)方差即式(2-224)推導(dǎo)為(2-230)考慮到

，式中sx，p表示對協(xié)方差矩陣Sx進(jìn)行Cholesky分解后的因子的第p列，則式(2-230)可以表示為(2-231)同樣，我們可得到一階估計互協(xié)方差矩陣為(2-232)上式也同樣可以寫為(2-233)可以看出，區(qū)間長度對于一階估計均值沒有影響，但是卻對一階協(xié)方差和互協(xié)方差有影響。分析表明，區(qū)間長度h最優(yōu)值的選取受到Δz分布的影響?？勺C明h與其峰值存在關(guān)系h2=σ4。

2.二階近似

采用二階截斷插值公式，可以得到f的更加精確的均值和協(xié)方差的逼近值。其二階截斷插值公式為~(2-234)由于Δz的均值為零，且其各個元素是互不相關(guān)的，可以得到f的二階估計期望為~(2-235)上式可被寫為(2-236)可以獲取協(xié)方差的估計值為(2-237)那么，可以得到二階協(xié)方差估計值為(2-238)式(2-238)中的奇數(shù)矩陣均值為零，它的第一項已經(jīng)在一階近似中研究過，現(xiàn)在就來看它的第二項和第三項。第二項由如下三項構(gòu)成(2-239)(2-240)(2-241)第三項由如下兩項構(gòu)成(2-242)(2-243)式(2-240)和式(2-243)是相等的，兩者相互可以抵消。另外，考慮到隨著向量z維數(shù)的增加，式(2-241)的計算量也會大大增加，為了降低計算量，將其忽略。再者，也是因為利用二階插值公式無法計算出所有的四階矩，就只能用三階矩逼近函數(shù)f。因此，二階協(xié)方差近似式為(2-244)考慮到σ2=1，且令h2=σ4(對于高斯分布σ4=3)，上式可以表示為(2-245)由于(2-246)即σ4≥σ22對任何分布都存在，因此可以選擇h2≥1。顯然，這意味著協(xié)方差估計總是半正定的。二階互協(xié)方差估計Pxy的推導(dǎo)為(2-247)2.8.3中心差分卡爾曼濾波

考慮非線性系統(tǒng)模型(2-248)(2-249)假定vk，wk是獨(dú)立同分布的，并獨(dú)立于當(dāng)前和過去的狀態(tài)，且有vk~(vk，Q(k))，wk~(wk，R(k))。狀態(tài)向量xk的一步預(yù)測的均值和協(xié)方差定義為－－(2-250)(2-251)其中，Yk－1是一個包含有過去量測值的矩陣，即(2-252)為方便起見，狀態(tài)估計的更新假定為線性的。令狀態(tài)估計的估計誤差最小化，可得(2-253)(2-254)其中(2-255)(2-256)(2-257)相應(yīng)地，狀態(tài)向量更新后的協(xié)方差矩陣為(2-258)采用一階多項式近似方法，得到的狀態(tài)轉(zhuǎn)移和量測方程如下(2-259)

(2-260)其中(2-261)(2-262)在上述近似方程的基礎(chǔ)上，可以獲得下列濾波步驟。時間更新步驟為(2-263)(2-264)(2-265)量測更新步驟為(2-266)(2-267)(2-268)在接下來的部分，將會介紹采用插值公式的方法來獲取非線性系統(tǒng)的狀態(tài)估計值。

1.一階CDKF

下面我們將運(yùn)用之前所介紹的一階中心差分逼近方法來獲取非線性系統(tǒng)的狀態(tài)估計值。首先，引入以下四個Choleskey分解(2-269)令表示的第j列，其它類似向量也采用這樣的方法表示。四個包含差分因子的矩陣定義為(2-270)(2-271)(2-272)(2-273)考慮一個包含有狀態(tài)向量和過程噪聲的擴(kuò)維狀態(tài)向量為(2-274)由于假定過程噪聲獨(dú)立于狀態(tài)向量，那么Δ的協(xié)方差為(2-275)

是隨機(jī)解耦的，引入z，使得。那么，不難看出狀態(tài)估計問題可以映射到如前所述的一般化的向量函數(shù)f(z)上。使用式(2-229)進(jìn)行狀態(tài)向量的一步更新~(2-276)在式(2-231)的基礎(chǔ)上，通過應(yīng)用式(2-270)~式(2-273)定義的矩陣，協(xié)方差更新為

(2-277)由于vk、xk相互獨(dú)立，協(xié)方差更新可以寫為兩個矩陣積之和的形式。上述計算過程中，協(xié)方差矩陣可能會出現(xiàn)非對稱和非正定性，從而帶來數(shù)值計算問題。與卡爾曼濾波類似，通常采用因子分解的方法加以解決。由于式(2-277)的計算是兩個二次項的和，因而不會出現(xiàn)數(shù)值計算問題。但是，在量測更新過程中需要用到分解因子，通過對下面復(fù)合矩陣的Choleskey分解獲取分解因子(2-278)由于該矩陣是矩形矩陣，為了后面的使用，需要將其轉(zhuǎn)換為方陣Choleskey因子，這可以通過Householder三角化方法獲得。

量測估計值為(2-279)相應(yīng)的復(fù)合矩陣為(2-280)式(2-280)是式(2-279)誤差協(xié)方差的Cholekey因子，即(2-281)對于矩陣，需要采用Householder三角化方法將其都轉(zhuǎn)換為方陣。為了計算狀態(tài)向量和量測估計之間的互協(xié)方差矩陣，使用式(2-233)中的結(jié)果得

(2-282)依據(jù)式(2-253)，卡爾曼增益為(2-283)依據(jù)式(2-254)，狀態(tài)向量更新為(2-284)后驗協(xié)方差可以通過式(2-258)更新，然而為了避免可能出現(xiàn)的數(shù)值計算問題，可以直接使用Cholekey分解因子進(jìn)行更新，則有下式(2-285)因此，量測更新可以表達(dá)為(2-286)顯然協(xié)方差矩陣的Cholekey分解因子方陣可以通過復(fù)合矩陣的三角化分解獲得(2-287)

2.二階CDKF

二階中心差分濾波器可以通過前面二階近似中的均值和協(xié)方差估計得到。首先定義四個包含有中心差分因子的矩陣(2-288)(2-289)(2-290)(2-291)如同一階中心差分濾波處理過程一樣，我們可以通過式(2-236)來獲取狀態(tài)估計方程(2-292)其中，nx表示狀態(tài)向量的維數(shù)，nv表示過程噪聲向量的維數(shù)。

先驗協(xié)方差的Cholekey因子同樣可以通過以下復(fù)合矩陣獲取(2-293)使用和一階中心差分濾波相同的方法，量測估計及其協(xié)方差分別為(2-294)(2-295)其中，nw表示量測噪聲向量的維數(shù)?；f(xié)方差矩陣為(2-296)卡爾曼增益為(2-297)狀態(tài)向量的量測更新為(2-298)依據(jù)式(2-286)協(xié)方差矩陣，可得狀態(tài)向量后驗更新的協(xié)方差矩陣為(2-299)顯然其含有Cholekey分解(2-300)

2.9小結(jié)

本章在卡爾曼濾波的基礎(chǔ)上，討論處理非線性系統(tǒng)濾波算法，主要涉及一些常用算法和國際上近年來提出的與卡爾曼濾波相關(guān)的非線性濾波算法。討論的算法有：擴(kuò)展卡爾曼濾波、不敏卡爾曼濾波、積分卡爾曼濾波、容積卡爾曼濾波、傅立葉厄米特卡爾曼濾波、中心差分濾波。粒子濾波算法也是一類重要的非線性系統(tǒng)濾波方法，放在下一章討論。第3章粒子濾波方法3.1引言3.2貝葉斯濾波3.3貝葉斯重要性采樣3.4序貫重要性重采樣粒子濾波算法3.5馬爾可夫鏈蒙特卡羅粒子濾波算法3.6輔助粒子濾波算法3.7正則化粒子濾波算法3.8邊緣粒子濾波算法3.9擴(kuò)展卡爾曼粒子濾波算法3.10高斯和粒子濾波算法3.11小結(jié)

3.1引言

粒子濾波是指通過尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本，對概率密度函數(shù)p(xk|yk)進(jìn)行近似，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)的最小方差估計的一種算法。粒子濾波算法依據(jù)系統(tǒng)狀態(tài)向量的先驗分布在狀態(tài)空間中產(chǎn)生一組隨機(jī)樣本，然后根據(jù)觀測量不斷地調(diào)整粒子的權(quán)值和位置，通過調(diào)整后的粒子信息修正最初的后驗概率函數(shù)。用數(shù)學(xué)語言可描述為：針對平穩(wěn)的時變系統(tǒng)，假定k－1時刻系統(tǒng)的后驗概率密度為p(xk－1|yk－1)，依據(jù)一定規(guī)則選取N個隨機(jī)樣本點(diǎn)，在k時刻獲得量測信息后,經(jīng)過狀態(tài)更新和時間更新過程,N個粒子的后驗概率密度近似為p(xk|yk),隨著粒子數(shù)的增加，粒子的概率密度函數(shù)就能逼近狀態(tài)真實(shí)的概率密度函數(shù)，對狀態(tài)向量的估計結(jié)果與最優(yōu)貝葉斯估計結(jié)果接近。粒子濾波適用于非線性非高斯系統(tǒng)的狀態(tài)估計，突破了傳統(tǒng)卡爾曼濾波理論框架，精度可以逼近最優(yōu)估計，是一種有效的非線性濾波技術(shù)，廣泛應(yīng)用于數(shù)字通信、圖像視頻處理、計算機(jī)視覺、語音信號處理、機(jī)器學(xué)習(xí)等領(lǐng)域。

3.2貝葉斯濾波

對于跟蹤問題，目標(biāo)狀態(tài)序列的狀態(tài)轉(zhuǎn)移可以用下面的目標(biāo)狀態(tài)序列{xk,k∈N}的演變方程來描述(3-1)其中，

是關(guān)于狀態(tài)xk－1的非線性函數(shù)，是獨(dú)立同分布的過程噪聲序列，nx,nv分別是狀態(tài)和過程噪聲向量的維數(shù)。系統(tǒng)量測方程為(3-2)從貝葉斯估計觀點(diǎn)來看，跟蹤問題就是計算k時刻狀態(tài)xk的某種置信程度。從1到k時刻，由于給定量測數(shù)據(jù)z1:k的值不同，得出的xk值也不同，因此需要構(gòu)造概率密度函數(shù)p(xk|z1:k)。假定初始概率密度函數(shù)p(x0|z0)≡p(x0)，x0表示初始狀態(tài)向量，z0表示尚且沒有獲得量測值，它也是先驗概率密度函數(shù)。因此從形式上看，通過預(yù)測和更新兩個步驟就可以遞推地得到概率密度函數(shù)p(xk|z1:k)的值。

假定k－1時刻的概率密度函數(shù)已知，那么通過ChapmanKolmogorov等式及使用模型(3-1)就可以預(yù)測出k時刻狀態(tài)的先驗概率密度函數(shù)(3-3)在等式(3-3)中，p(xk|xk－1)=p(xk|xk－1，z1:k－1)，且滿足等式(3-1)所描述的一階馬爾可夫過程。狀態(tài)估計p(xk|xk－1)的概率模型由系統(tǒng)等式(3-1)和統(tǒng)計值vk－1來確定。在k時刻，可以得到量測值z=，然后通過貝葉斯規(guī)則更新先驗概率密度函數(shù)(3-4)式中的常量(3-5)是由模型(3-2)定義的似然函數(shù)p(zk|xk)和一步預(yù)測的統(tǒng)計值p(xk|z1:k－1)共同確定的。在更新式子(3-4)中，量測值zk被用來修正先驗概率，以獲得當(dāng)前狀態(tài)的后驗概率。式(3-3)和式(3-4)是最優(yōu)貝