機器視覺：6.運動估計與濾波

目標跟蹤

光流估計背景提取運動模型與濾波背景提取背景建?；舅枷?/p>

基于單一高斯模型的背景建?；旌细咚鼓Ｐ突诨旌细咚鼓Ｐ偷谋尘敖?.1背景建模基本思想Part14當相機靜止時，圖像采集場景保持不變，因此圖像中的每個像素可分為兩類：背景和前景，其中前景對應運動目標。只要確定像素類別，進一步就可利用之前的圖像標記等方法對運動目標前景進行進一步的處理分析。背景建模的核心問題：如何在存在光照干擾、不同目標運動速度的情況下準確對各像素進行分類背景建模基本思想1.1背景建?；舅枷隤art15基本背景建模方法：幀間差分法原理：將基于時間序列圖像中相鄰兩幀或幾幀圖像逐個像素進行對比，得到一幅差值圖像；再通過事先確定的閾值對差值圖像進行二值化處理，如果大于指定閾值，則認為是前景。當使用兩幀圖像時，幀間差分法可表示為：背景建?；痉椒ā獛g差分法1.1背景建模基本思想Part16幀間差分法

實例優(yōu)點：計算簡單、計算量小、對光線變化不敏感缺點：容易造成實體內(nèi)部空洞，差分間隔需要根據(jù)運動目標的速度選擇，選擇不當會影響背景提取精確度。背景建?；痉椒ā獛g差分法1.2基于單一高斯模型的背景建模Part17

單一高斯模型——基本思想1.2基于單一高斯模型的背景建模Part18正態(tài)分布曲線呈對稱形態(tài)，由兩個參數(shù)決定：均值和標準差。均值決定正態(tài)曲線峰值的位置，表示數(shù)據(jù)的集中趨勢；標準差則決定正態(tài)曲線的形狀。當較大時，曲線更為平緩，表示數(shù)據(jù)分散程度較高；而較小時，曲線更陡峭，表示數(shù)據(jù)分布較為緊密。對于標準正態(tài)分布曲線，其與x軸圍成的面積為1。單一高斯模型——基本思想1.2基于單一高斯模型的背景建模Part19

單一高斯模型——基本思想1.2基于單一高斯模型的背景建模Part110

單一高斯模型——基本思想1.2基于單一高斯模型的背景建模Part111

單一高斯模型——基本思想1.2基于單一高斯模型的背景建模Part112

單一高斯模型——改進方法1.2基于單一高斯模型的背景建模Part113

單一高斯模型——多元高斯模型1.2基于單一高斯模型的背景建模Part114Σ是協(xié)方差矩陣。

當處理彩色或其他類型多通道圖像時，可將每一像素視為多維向量，使用多元高斯模型建模。此時背景的判別需對每一分量依次進行；所有分量滿足3σ準則時則認為其屬于背景。單一高斯模型——多元高斯模型1.3混合高斯模型Part115簡單場景使用單一高斯模型，處理速度快，分割對象比較完整。

復雜背景像素值可能在多個亮度區(qū)間跳變，不能用單一高斯分布建模。1.3混合高斯模型Part116混合高斯模型：將觀測數(shù)據(jù)的分布視為多個高斯分布的線性組合每個高斯分布代表數(shù)據(jù)中的一個子集或類別，這些子集可能對應于不同的背景狀態(tài)或運動模式。每個分布均可用多個高斯分布線性組合描述。1.3混合高斯模型Part117把高斯分布函數(shù)視為高斯基，幾乎任意形狀的概率密度函數(shù)均可以通過在高斯基上進行線性展開來近似，即混合高斯模型具有普遍適用性。1.4基于混合高斯模型的背景建模Part118混合高斯模型背景建模方法由Stauffer等提出。該方法為每個像素建立Q個高斯模型，一般Q取3～5，使模型克服外界環(huán)境影響。在背景無運動物體的時候，連續(xù)采N幀圖像，用來實現(xiàn)對背景模型的初始化?；旌细咚鼓Ｐ捅尘敖！舅枷?.4基于混合高斯模型的背景建模Part119

混合高斯模型背景建?！舅枷?.4基于混合高斯模型的背景建模Part120

混合高斯模型背景建模——算法步驟1.4基于混合高斯模型的背景建模Part121

混合高斯模型背景建?！惴ú襟E1.4基于混合高斯模型的背景建模Part122

混合高斯模型背景建?！惴ú襟E1.4基于混合高斯模型的背景建模Part123

混合高斯模型背景建?！惴ú襟E1.4基于混合高斯模型的背景建模Part124

混合高斯模型背景建?！惴ú襟E1.4基于混合高斯模型的背景建模Part125背景提取效果示意圖如圖，測試所用視頻采集來自于某路口拍攝圖像序列，攝像機固定安裝在道路右側(cè)車道上方。使用前述MoG背景提取算法對背景進行學習，學習速率α=0.002?；旌细咚鼓Ｐ捅尘敖！Ч?.4基于混合高斯模型的背景建模Part126混合高斯模型背景建模——效果光流估計基本光流方程Lucas-Kanade算法金字塔光流法Horn-Schunck算法2.1基本光流方程Part228人們觀察三維世界中目標的運動時，運動目標的輪廓會在人的視網(wǎng)膜上形成一系列連續(xù)變化的圖像，這些連續(xù)變化

的信息不斷地“流過”人眼視網(wǎng)膜（即圖像平面），就像一種光“流”過一樣，稱之為光流（opticalflow）。在視頻跟蹤領(lǐng)域，光流是指圖像中灰度模式的表面運動，是物體的三維速度矢量在像平面上的投影，它表示物體在圖像中位置的瞬時變化。光流來源于仿生學思想，用于估計圖像序列中的每個像素在時間上的移動方向和速度。基本光流方程——基本思想2.1基本光流方程Part229光流法的基本假設是，相鄰幀之間圖像中像素的灰度值在短時間內(nèi)是保持穩(wěn)定的，即它們的移動量相對于幀間的時間變化而言很小。通過對相鄰幀之間的像素灰度值進行比較，可以計算出每個像素的移動向量，即該像素從當前幀到下一幀的位移量。計算得到的位移量可用于運動估計、視頻穩(wěn)定、目標跟蹤等視覺應用。基本光流方程——基本思想2.1基本光流方程Part230

基本光流方程——基本思想2.1基本光流方程Part231由此可以得到上式按照泰勒公式展開可得當位移量dx，dy較小時，忽略二次及高次項，整理可得

基本光流方程——基本思想2.1基本光流方程Part232

基本光流方程——基本思想2.1基本光流方程Part233

基本光流方程——基本思想2.2Lucas-Kanade算法Part234Lucas-Kanade算法，簡稱L-K算法，最初于1981年由Lucas和Kanade二人提出。該算法假設在一個小的空間鄰域內(nèi)運動矢量保持恒定，進一步使用加權(quán)最小二乘法估計光流。由于該算法應用于輸入圖像的一組點上時比較方便，因此被廣泛應用于稀疏光流場計算。Lucas-Kanade算法——基本思想2.Lucas-Kanade算法Part235Lucas-Kanade算法，簡稱L-K算法，最初于1981年由Lucas和Kanade二人提出。該算法假設在一個小的空間鄰域內(nèi)運動矢量保持恒定，進一步使用加權(quán)最小二乘法估計光流。由于該算法應用于輸入圖像的一組點上時比較方便，因此被廣泛應用于稀疏光流場計算。Lucas-Kanade算法——基本思想2.2Lucas-Kanade算法Part236L-K算法的提出基于以下三個假設。亮度恒定不變。目標像素在不同幀間運動時特征保持不變，對于灰度圖，假設在整個被跟蹤期間像素亮度不變。時間連續(xù)或者運動是“小運動”。相鄰幀間的像素運動較小，圖像運動相對時間來說比較緩慢。空間一致。同一場景中同一表面上的鄰近點運動情況相似，且這些點在圖像上的投影也在鄰近區(qū)域。Lucas-Kanade算法——假設2.2Lucas-Kanade算法Part237

Lucas-Kanade算法——估計光流2.2Lucas-Kanade算法Part238

Lucas-Kanade算法——估計光流2.2Lucas-Kanade算法Part239其中，Lucas-Kanade算法——估計光流2.2Lucas-Kanade算法Part240當

A滿秩時，令

，可得最小二乘解：其中，+代表矩陣的Moore-Penrose廣義逆。Lucas-Kanade算法——估計光流2.2Lucas-Kanade算法Part241對一個旋轉(zhuǎn)的魔方使用L-K算法進行運動估計。（a）、（b）為魔方旋轉(zhuǎn)序列中的兩幅圖像。（c）是運動估計后的結(jié)果。Lucas-Kanade算法——結(jié)果演示2.3金字塔光流法Part242當像素運動較大時，

假設無法滿足，需對原算法進行改進。金字塔光流法可對這一問題進行有效處理。金字塔光流法使用圖像金字塔分解技術(shù)，將原始圖像分解成多個尺度，每個尺度都是原始圖像的縮小版本，然后在每個尺度上運行Lucas-Kanade算法，以獲得更準確的光流結(jié)果。金字塔光流法——基本思想2.3金字塔光流法Part243對原始圖像進行金字塔分解，生成多個尺度上的圖像，通常上一層的尺度為下一層的二分之一。在最粗糙的尺度上運行L-K算法，得到初始光流估計。從粗糙的尺度開始，向上逐步升級到較細的尺度。在每個尺度上，使用前一尺度的光流估計作為初始估計，并在該尺度上運行L-K算法，以得到更精確的光流估計。將每個尺度上的光流估計組合起來，得到最終的光流向量。金字塔光流法——算法步驟2.3金字塔光流法Part244

金字塔光流法——算法步驟2.3金字塔光流法Part245對一個旋轉(zhuǎn)的魔方使用金字塔光流法進行運動估計。金字塔光流法——結(jié)果演示2.4Horn-Schunck算法Part246Horn-Schunck算法（簡稱H-S算法）的基本思想是在基本光流方程基礎上引入全局光流平滑約束假設，假設在整幅圖像上光流的變化是光滑的，即物體運動矢量是平滑的或緩慢變化的，利用這一條件，在光流方程

的基礎上加入約束并構(gòu)造指標泛函，使光流求解問題正則化。Horn-Schunck算法——基本思想2.4Horn-Schunck算法Part247

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part248

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part249

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part250合并前兩個式子得：Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part251

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part252

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part253

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part254

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part255

Horn-Schunck算法——具體步驟2.4Horn-Schunck算法Part256Horn-Schunck算法——算法結(jié)果2.4Horn-Schunck算法Part257Horn-Schunck算法——算法結(jié)果H-S算法可產(chǎn)生更為稠密的光流估計結(jié)果，但L-K算法的結(jié)果已基本反映實際運動情況，同時對噪聲（兩幅圖像背景處）更為穩(wěn)健。目標跟蹤基本問題Meanshift算法KCF算法3.1基本問題Part359目標跟蹤是一種實時自動跟蹤連續(xù)圖像序列或視頻中特定目標的方法，旨在確定目標在每個時刻的位置信息。其主要任務為，給定第k幀圖像中目標的位置，輸出目標在第k+1幀中的位置。如圖（a）中被檢測到的車輛在圖（b）中對應的位置由方框標出，不同車輛對應關(guān)系通過左上角編號標識。（a）原圖像（b）追蹤結(jié)果3.1基本問題Part360目標跟蹤的難點在于需要適應各種不同的環(huán)境、背景及不斷變化的目標形態(tài)，同時滿足實時性要求。光線變化和目標外觀的改變可能導致目標丟失，或者產(chǎn)生目標分離或連接等不良后果。此外，長時間遮擋、復雜背景、相機運動、光照變化及持續(xù)跟蹤等也是目標跟蹤需要考慮的因素。同時，復雜度過高的算法可能無法滿足實時性要求。因此，研究一種高實時性、高精度和高魯棒性的目標跟蹤算法顯得尤為重要。3.2Meanshift算法Part361Meanshift算法是一種基于均值漂移的目標跟蹤方法，通過計算像素特征概率來比較上一幀的目標模型與當前幀的候選模型。該算法利用相似性度量函數(shù)選取最相似的候選模型，沿著得到的Meanshift向量移動目標位置，并通過不斷迭代使算法收斂到目標位置。

Meanshift作為一種多功能數(shù)據(jù)分析方法，被應用于眾多領(lǐng)域。它旨在從數(shù)據(jù)密度分布中尋找局部最大值。對于連續(xù)數(shù)據(jù)分布，處理過程相對簡單，僅需將爬山算法應用于數(shù)據(jù)密度直方圖。由于Meanshift算法在統(tǒng)計意義上排除了數(shù)據(jù)中的離群值，即與數(shù)據(jù)峰值距離較遠的點，它被視為一種穩(wěn)定的方法。該算法僅處理局部窗口內(nèi)的數(shù)據(jù)點，并在處理結(jié)束后轉(zhuǎn)移窗口。Meanshift算法——基本思想3.2Meanshift算法Part362Meanshift算法包含以下步驟。選定搜索窗口，包括窗口的初始位置、種類（均勻、多項式、指數(shù)、高斯）、形態(tài)（對稱、非對稱、旋轉(zhuǎn)、圓形、矩形）及尺寸（窗口外的數(shù)據(jù)將被忽略）。計算帶權(quán)重窗口的質(zhì)心。將窗口中心置于計算出的質(zhì)心位置?；氐降冖诓剑貜蛨?zhí)行，直至窗口位置保持不變（通常會達到這一狀態(tài)）。Meanshift算法——算法步驟3.2Meanshift算法Part363

Meanshift算法——算法步驟3.2Meanshift算法Part364Meanshift算法與核密度估計息息相關(guān)。在這里，“核”是一種局部函數(shù)，如高斯分布。如果有足夠多的點上具有適當?shù)臋?quán)重和尺度的核，數(shù)據(jù)分布就可以完全依據(jù)這些核進行描述。與核密度估計不同，Meanshift算法只估計數(shù)據(jù)分布的梯度（即變化方向）。梯度為零的位置一般表示分布的頂峰（雖然可能是局部的）。當然，在其他位置或尺度上也可能存在頂峰。Meanshift算法——核密度估計3.2Meanshift算法Part365Meanshift算法中的核心公式就是矩陣的核，它將Meanshift向量等式簡化為計算圖像像素分布的重心：其中，零階矩和一階矩的計算方法如下：Meanshift算法——核密度估計3.2Meanshift算法Part366計算得到的Meanshift向量告訴我們?nèi)绾螌eanshift窗口的中心重新移動到由計算得出的此窗口的重心的位置。顯然，窗口的移動造成了窗口內(nèi)容的改變，于是我們又重復剛才重新定位窗口中心的步驟。窗口中心重定位的過程通常會收斂到Meanshift向量為0（也就是窗口不再移動）。收斂的位置在窗口中像素分布的局部最大值（峰值）處。由于峰值本身是一個對尺度變化敏感的量，因此窗口的大小不同，峰值的位置也不一樣。Meanshift算法——核密度估計3.2Meanshift算法Part367Meanshift算法——算法流程圖3.2Meanshift算法Part368Meanshift算法——效果演示

（a）Meanshift追蹤圖片1

（b）Meanshift追蹤圖片23.2Meanshift算法Part369Meanshift算法——效果演示

（a）Meanshift追蹤圖片1

（b）Meanshift追蹤圖片23.3KCF

算法Part370KCF（核相關(guān)濾波）算法是圖像跟蹤的一類快速有效的方法，可在背景運動甚至劇烈干擾時仍有效完成實時目標跟蹤。其基本思想是，在當前圖像中找到與待跟蹤目標最“相關(guān)”的區(qū)域；“相關(guān)”可以通過良好設計的相關(guān)濾波器實現(xiàn)，即設計一個濾波模板，使得當它作用在跟蹤目標上時，得到的響應最大。KCF算法——基本思想3.3KCF

算法Part371

KCF算法——基本思想3.3KCF

算法Part372KCF算法中的兩個關(guān)鍵問題是，如何設計濾波模板，確保在目標變化及環(huán)境干擾時仍具有很好的跟蹤性能；如何確定輸出“最大”的相應位置。KCF使用核相關(guān)濾波器實現(xiàn)跟蹤算法，核相關(guān)濾波器是目前最為成功的跟蹤器之一，在公開數(shù)據(jù)集上有著很好的跟蹤效果，同時速度快，便于實時跟蹤處理。KCF算法——基本思想3.3KCF

算法Part373

KCF算法——嶺回歸3.3KCF

算法Part374

KCF算法——嶺回歸3.3KCF

算法Part375

KCF算法——嶺回歸3.3KCF

算法Part376當信號為二維圖像時，則移動包含x和y方向，如圖所示。其中，圖（c）為基準樣本，其他為基準樣本移動不同位移后產(chǎn)生的樣本。KCF算法——嶺回歸3.3KCF

算法Part377

KCF算法——嶺回歸3.3KCF

算法Part378因此，上述線性回歸的解的傅里葉變換可以表示為KCF算法——嶺回歸3.3KCF

算法Part379在實際應用中，嶺回歸算法難以解決復雜輸入導致的線性不可分的問題。為了解決這一問題。需要使用非線性回歸的方法，將原始的特征空間映射到高維甚至無窮維的空間中，從而使得原本線性不可分的數(shù)據(jù)變?yōu)榫€性可分。KCF算法——非線性回歸3.3KCF

算法Part380然而在高維空間Z下計算內(nèi)積十分復雜，如果升維到無窮維，甚至會無法計算。針對這一問題，可以使用核技巧通過核函數(shù)隱式地將

X

空間映射到Z空間，從而實現(xiàn)內(nèi)積的快速運算，如下所示：其中，

為內(nèi)積運算；

為核函數(shù)，其選取原則就是使樣本在升維后的空間中變得線性可分。KCF算法——非線性回歸3.3KCF

算法Part381

KCF算法——非線性回歸3.3KCF

算法Part382

KCF算法——非線性回歸3.3KCF

算法Part383

KCF算法——非線性回歸3.3KCF

算法Part384

KCF算法——非線性回歸3.3KCF

算法Part385當輸入圖像為多通道（彩色圖、特征圖）時，核相關(guān)的計算公式為：

KCF算法——非線性回歸3.3KCF

算法Part386

KCF算法——算法步驟3.3KCF

算法Part387

KCF算法——非線性回歸3.3KCF

算法Part388

KCF算法——非線性回歸3.3KCF

算法Part389

KCF算法——應用示例運動模型與濾波運動模型卡爾曼濾波粒子濾波4.1運動模型Part491在目標檢測跟蹤系統(tǒng)中，需要建立被跟蹤對象合適的運動模型，進一步對運動模型參數(shù)進行估計，并對未來狀態(tài)進行預測。本節(jié)先介紹常用的運動模型，再介紹卡爾曼濾波和粒子濾波的基礎理論與基本原理。4.1運動模型Part492在實際中，目標的運動軌跡經(jīng)常用二次（勻速運動）和三次（勻加速運動）的多項式模型描述。恒速運動模型是常用模型之一，即假設相鄰幀時間間隔足夠短，目標運動速度近似不變。設特征坐標

，則勻速運動可由下式描述：記速度為

。對方程積分可得4.1運動模型Part493定義狀態(tài)向量可得離散狀態(tài)方程式中4.1運動模型Part494事實上，速度不可能一直保持恒定。設

時刻速度增量為

，則位置增量為

。因此，目標運動模型模型可用下列狀態(tài)方程描述：式中4.1運動模型Part495系統(tǒng)觀測狀態(tài)由目標識別跟蹤算法確定，觀測矩陣為：增量

和量測噪聲

可描述為零均值白噪聲序列，則

，

應該和

在同一數(shù)量級，實際中一般取

。

定義為各元素絕對值的最大值，根據(jù)目標運動速度決定。

由目標檢測算法精度決定。4.2卡爾曼濾波Part496卡爾曼濾波（KalmanFiltering）是一種經(jīng)典的最優(yōu)狀態(tài)估計與濾波方法，它可以處理包含噪聲的量測數(shù)據(jù)，以提取出真實的狀態(tài)信息?？柭鼮V波的核心思想是利用系統(tǒng)的動態(tài)模型和觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行預測和校正，即利用前一時刻的估計值和現(xiàn)時刻的量測值來更新對狀態(tài)變量的估計，求出現(xiàn)時刻的估計值。由于其原理清晰，易于實現(xiàn)，并且具有最小方差誤差估計特性，因此在運動跟蹤領(lǐng)域得到了廣泛應用。

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part497卡爾曼濾波包含預測與更新兩個步驟。其中，預測步驟可以理解為根據(jù)系統(tǒng)過去狀態(tài)和估計系統(tǒng)模型，推導此刻系統(tǒng)的狀態(tài)；更新步驟可以理解為比較此刻預測的系統(tǒng)狀態(tài)和傳感器的量測值，對預測的系統(tǒng)狀態(tài)進行修正。

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part498

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part499

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part4100

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part4101

卡爾曼濾波——線性卡爾曼濾波4.3粒子濾波Part4102

粒子濾波——基本原理4.2卡爾曼濾波Part4103

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part4104

卡爾曼濾波——線性卡爾曼濾波4.2卡爾曼濾波Part4105卡爾曼濾波——線性卡爾曼濾波流程4.2卡爾曼濾波Part4106

卡爾曼濾波——線性卡爾曼濾波實例4.2卡爾曼濾波Part4107根據(jù)卡爾曼濾波基本方程得到對狀態(tài)的估計值?？柭鼮V波——線性卡爾曼濾波實例4.2卡爾曼濾波Part4108經(jīng)典的卡爾曼濾波僅適用于線性系統(tǒng)，且誤差應符合理想高斯分布。然而在實際中，系統(tǒng)的狀態(tài)和測量模型不再是線性的，例如，狀態(tài)方程是非線性的或者觀測與狀態(tài)之間的關(guān)系是非線性的，這時標準卡爾曼濾波不再適用。在實際應用里，解決非線性濾波問題往往采用各種線性近似的方法，將非線性關(guān)系進行線性近似，將其轉(zhuǎn)化成線性問題?？柭鼮V波——非線性卡爾曼濾波4.2卡爾曼濾波Part4109非線性卡爾曼濾波對標準卡爾曼濾波算法進行了擴展，使之能處理非線性模型。擴展卡爾曼濾波（ExtendedKalmanFiltering，EKF）就是一種使用廣泛的非線性卡爾曼濾波方法，對非線性狀態(tài)轉(zhuǎn)移函數(shù)和非線性觀測函數(shù)進行一階泰勒展開，并用得到的一階近似項作為原狀態(tài)方程和觀測方程近似表達形式，從而實現(xiàn)線性化，同時假定線性化后的狀態(tài)依然服從理想高斯分布，最后對線性化后的系統(tǒng)采用標準卡爾曼濾波獲得狀態(tài)估計。盡管比線性卡爾曼濾波更復雜，但它可以更準確地估計非線性系統(tǒng)狀態(tài)，因此應用場景更為廣泛?？柭鼮V波——非線性卡爾曼濾波4.2卡爾曼濾波Part4110

卡爾曼濾波——非線性卡爾曼濾波4.2卡爾曼濾波Part4111和線性系統(tǒng)的情況一樣，我們可以得到擴展卡爾曼濾波算法如下：卡爾曼濾波——非線性卡爾曼濾波4.2卡爾曼濾波Part4112

卡爾曼濾波——非線性卡爾曼濾波4.2卡爾曼濾波Part4113二維平面小車的運動估計，采用一個簡單的非線性勻轉(zhuǎn)速勻速度（CTRV）模型來描述小車，假設觀察到的狀態(tài)變量為：分別代表平面上的二維空間坐標、速度、角速度和轉(zhuǎn)速，如下圖所示卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4114卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4115可得到系統(tǒng)的狀態(tài)轉(zhuǎn)移方程：卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4116假設可以觀測的狀態(tài)量為二維平面坐標：則觀測矩陣為：卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4117同時也可得到系統(tǒng)的雅可比矩陣和系統(tǒng)誤差分別為：卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4118將上述模型代入擴展卡爾曼濾波算法，為了對比效果，分別采用上述CTRV模型與EKF算法、線性勻速度（CV）模型與標準卡爾曼濾波算法進行驗證。得到的仿真結(jié)果如下圖所示，其中，紅色軌跡為小車真實軌跡，綠色實線為CTRV模型估計小車軌跡，藍色實線為CV模型估計小車的軌跡?？梢钥闯?，CTRV模型結(jié)合EKF算法的預測效果更優(yōu)，更貼近真值。卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4119卡爾曼濾波——非線性卡爾曼濾波實例4.2卡爾曼濾波Part4120卡爾曼濾波——非線性卡爾曼濾波實例4.3粒子濾波Part4121非線性卡爾曼濾波可進行非線性運動預測，但要求系統(tǒng)狀態(tài)和測量函數(shù)連續(xù)可微，實際系統(tǒng)可能無法滿足。由于對非線性函數(shù)進行了一階近似，同時需要計算雅可比矩陣，容易產(chǎn)生精度下降甚至發(fā)散的問題。粒子濾波則為解決這些問題提供了一類有效的方法。粒子濾波——基本原理4.3粒子濾波Part4122粒子濾波（ParticleFilter，PF）建立在蒙特卡洛模擬方法的基礎上。蒙特卡洛模擬指利用所求狀態(tài)空間中大量的樣本點來近似逼近待估計變量的后驗概率分布，從而將積分問題轉(zhuǎn)換為有限樣本點的求和問題。然而在實際計算中，通常無法直接從后驗概率分布中采樣，如何得到服從后驗概率分布的隨機樣本是蒙特卡洛模擬方法中的基本問題之一。為解決上述問題，重要性采樣法引入一個已知的、容易采樣的重要性概率密度函數(shù)并從中生成采樣粒子，利用這些隨機樣本的加權(quán)和來逼近后驗濾波概率密度。簡言之，粒子濾波算法是利用一系列隨機樣本的加權(quán)和表示后驗概率密度，通過求和來近似積分操作。粒子濾波——基本原理4.3粒子濾波Part4123

粒子濾波——基本原理4.3粒子濾波Part4124

粒子濾波——基本原理4.3粒子濾波Part4125

粒子濾波——基本原理4.3粒子濾波Part4126在基于重要性采樣的蒙特卡洛模擬方法中，估計后驗概率需要利用所有的觀測數(shù)據(jù)，每次新的觀測數(shù)據(jù)到來都需要重新計算整個狀態(tài)序列的重要性權(quán)值。序貫重要性采樣將統(tǒng)計學中的序貫分析方法應用到蒙特卡洛模擬方法中，從而實現(xiàn)后驗概率的遞推估計。

粒子濾波——基本原理4.3粒子濾波Part4127假設重要性概率密度函數(shù)

可以分解為

設系統(tǒng)狀態(tài)是一個馬爾可夫過程且給定系統(tǒng)狀態(tài)下各次觀測獨立，則有

粒子濾波——基本原理4.3粒子濾波Part4128其遞歸形式可以表示為