機(jī)器視覺(jué)：6.運(yùn)動(dòng)估計(jì)與濾波

目標(biāo)跟蹤

光流估計(jì)背景提取運(yùn)動(dòng)模型與濾波背景提取背景建?；舅枷?/p>

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

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

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

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

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

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

單一高斯模型——改進(jìn)方法1.2基于單一高斯模型的背景建模Part113

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

當(dāng)處理彩色或其他類(lèi)型多通道圖像時(shí)，可將每一像素視為多維向量，使用多元高斯模型建模。此時(shí)背景的判別需對(duì)每一分量依次進(jìn)行；所有分量滿(mǎn)足3σ準(zhǔn)則時(shí)則認(rèn)為其屬于背景。單一高斯模型——多元高斯模型1.3混合高斯模型Part115簡(jiǎn)單場(chǎng)景使用單一高斯模型，處理速度快，分割對(duì)象比較完整。

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

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

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

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

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

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

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

的信息不斷地“流過(guò)”人眼視網(wǎng)膜（即圖像平面），就像一種光“流”過(guò)一樣，稱(chēng)之為光流（opticalflow）。在視頻跟蹤領(lǐng)域，光流是指圖像中灰度模式的表面運(yùn)動(dòng)，是物體的三維速度矢量在像平面上的投影，它表示物體在圖像中位置的瞬時(shí)變化。光流來(lái)源于仿生學(xué)思想，用于估計(jì)圖像序列中的每個(gè)像素在時(shí)間上的移動(dòng)方向和速度?；竟饬鞣匠獭舅枷?.1基本光流方程Part229光流法的基本假設(shè)是，相鄰幀之間圖像中像素的灰度值在短時(shí)間內(nèi)是保持穩(wěn)定的，即它們的移動(dòng)量相對(duì)于幀間的時(shí)間變化而言很小。通過(guò)對(duì)相鄰幀之間的像素灰度值進(jìn)行比較，可以計(jì)算出每個(gè)像素的移動(dòng)向量，即該像素從當(dāng)前幀到下一幀的位移量。計(jì)算得到的位移量可用于運(yùn)動(dòng)估計(jì)、視頻穩(wěn)定、目標(biāo)跟蹤等視覺(jué)應(yīng)用?；竟饬鞣匠獭舅枷?.1基本光流方程Part230

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

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

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

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

Lucas-Kanade算法——估計(jì)光流2.2Lucas-Kanade算法Part238

Lucas-Kanade算法——估計(jì)光流2.2Lucas-Kanade算法Part239其中，Lucas-Kanade算法——估計(jì)光流2.2Lucas-Kanade算法Part240當(dāng)

A滿(mǎn)秩時(shí)，令

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

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

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

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

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

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

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

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

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

（a）Meanshift追蹤圖片1

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

（a）Meanshift追蹤圖片1

（b）Meanshift追蹤圖片23.3KCF

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

算法Part371

KCF算法——基本思想3.3KCF

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

算法Part373

KCF算法——嶺回歸3.3KCF

算法Part374

KCF算法——嶺回歸3.3KCF

算法Part375

KCF算法——嶺回歸3.3KCF

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

算法Part377

KCF算法——嶺回歸3.3KCF

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

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

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

X

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

為內(nèi)積運(yùn)算；

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

算法Part381

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

算法Part382

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

算法Part383

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

算法Part384

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

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

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

算法Part386

KCF算法——算法步驟3.3KCF

算法Part387

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

算法Part388

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

算法Part389

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

，則勻速運(yùn)動(dòng)可由下式描述：記速度為

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

時(shí)刻速度增量為

，則位置增量為

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

和量測(cè)噪聲

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

，

應(yīng)該和

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

。

定義為各元素絕對(duì)值的最大值，根據(jù)目標(biāo)運(yùn)動(dòng)速度決定。

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

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

卡爾曼濾波——線性卡爾曼濾波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

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

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

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

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

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

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

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

可以分解為

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

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