由于傳感器本身的特性，任何測(cè)量結(jié)果都是有誤差的。以障礙物檢測(cè)為例，如果直接使用傳感器的測(cè)量結(jié)果，在車(chē)輛顛簸時(shí)，可能會(huì)造成障礙物測(cè)量結(jié)果的突變，這對(duì)無(wú)人車(chē)的感知來(lái)說(shuō)是不可接受的。因此需要在傳感器測(cè)量結(jié)果的基礎(chǔ)上，進(jìn)行跟蹤，以此來(lái)保證障礙物的位置、速度等信息不會(huì)發(fā)生突變。

最經(jīng)典的跟蹤算法莫過(guò)于卡爾曼老爺子在1960年提出的卡爾曼濾波器。在無(wú)人車(chē)領(lǐng)域，卡爾曼濾波器除了應(yīng)用于障礙物跟蹤外，也在車(chē)道線跟蹤、障礙物預(yù)測(cè)以及定位等領(lǐng)域大展身手。

在介紹卡爾曼濾波器數(shù)學(xué)原理之前，先從感性上看一下它的工作原理。簡(jiǎn)單來(lái)講，卡爾曼濾波器就是根據(jù)上一時(shí)刻的狀態(tài)，預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)，將預(yù)測(cè)的狀態(tài)與當(dāng)前時(shí)刻的測(cè)量值進(jìn)行加權(quán)，加權(quán)后的結(jié)果才認(rèn)為是當(dāng)前的實(shí)際狀態(tài)，而不是僅僅聽(tīng)信當(dāng)前的測(cè)量值。

初始化（Initialization）

假設(shè)有個(gè)小車(chē)在道路上向右側(cè)勻速運(yùn)動(dòng)，我們?cè)谧髠?cè)安裝了一個(gè)測(cè)量小車(chē)距離和速度傳感器，傳感器每1秒測(cè)一次小車(chē)的位置s和速度v，如下圖所示。

我們用向量xt來(lái)表示當(dāng)前小車(chē)的狀態(tài)，該向量也是最終的輸出結(jié)果，被稱(chēng)作狀態(tài)向量（state vector）：

由于測(cè)量誤差的存在，傳感器無(wú)法直接獲取小車(chē)位置的真值，只能獲取在真值附近的一個(gè)近似值，可以假設(shè)測(cè)量值在真值附近服從高斯分布。如下圖所示，測(cè)量值分布在紅色區(qū)域的左側(cè)或右側(cè)，真值則在紅色區(qū)域的波峰處。

由于是第一次測(cè)量，沒(méi)有小車(chē)的歷史信息，我們認(rèn)為小車(chē)在1秒時(shí)的狀態(tài)x與測(cè)量值z(mì)相等，表示如下：

預(yù)測(cè)（Prediction）

預(yù)測(cè)是卡爾曼濾波器中很重要的一步，這一步相當(dāng)于使用歷史信息對(duì)未來(lái)的位置進(jìn)行推測(cè)。根據(jù)第1秒小車(chē)的位置和速度，我們可以推測(cè)第2秒時(shí)，小車(chē)所在的位置應(yīng)該如下圖所示。會(huì)發(fā)現(xiàn)，圖中紅色區(qū)域的范圍變大了，這是因?yàn)轭A(yù)測(cè)時(shí)加入了速度估計(jì)的噪聲，是一個(gè)放大不確定性的過(guò)程。

根據(jù)小車(chē)第一秒的狀態(tài)進(jìn)行預(yù)測(cè)，得到預(yù)測(cè)的狀態(tài)xpre：

其中，pre是prediction的簡(jiǎn)稱(chēng)；時(shí)間間隔為1秒，所以預(yù)測(cè)位置為距離+速度*1；由于小車(chē)做的是勻速運(yùn)動(dòng)，因此速度保持不變。

觀測(cè)（Measurement）

在第2秒時(shí)，傳感器對(duì)小車(chē)的位置做了一次觀測(cè)，我們認(rèn)為小車(chē)在第2秒時(shí)觀測(cè)值為z2，用向量表示第2秒時(shí)的觀測(cè)結(jié)果為：

很顯然，第二次觀測(cè)的結(jié)果也是存在誤差的，我們將預(yù)測(cè)的小車(chē)位置與實(shí)際觀測(cè)到的小車(chē)位置放到一個(gè)圖上，即可看到：

圖中紅色區(qū)域?yàn)轭A(yù)測(cè)的小車(chē)位置，藍(lán)色區(qū)域?yàn)榈?秒的觀測(cè)結(jié)果。

很顯然，這兩個(gè)結(jié)果都在真值附近。為了得到盡可能接近真值的結(jié)果，我們將這兩個(gè)區(qū)域的結(jié)果進(jìn)行加權(quán)，取加權(quán)后的值作為第二秒的狀態(tài)向量。

為了方便理解，可以將第2秒的狀態(tài)向量寫(xiě)成：

其中，w1為預(yù)測(cè)結(jié)果的權(quán)值，w2為觀測(cè)結(jié)果的權(quán)值。兩個(gè)權(quán)值的計(jì)算是根據(jù)預(yù)測(cè)結(jié)果和觀測(cè)結(jié)果的不確定性來(lái)的，這個(gè)不確定性就是高斯分布中的方差的大小，方差越大，波形分布越廣，不確定性越高，這樣一來(lái)給的權(quán)值就會(huì)越低。

加權(quán)后的狀態(tài)向量的分布，可以用下圖中綠色區(qū)域表示：

你會(huì)發(fā)現(xiàn)綠色區(qū)域的方差比紅色區(qū)域和藍(lán)色區(qū)域的小。這是因?yàn)檫M(jìn)行加權(quán)運(yùn)算時(shí)，需要將兩個(gè)高斯分布進(jìn)行乘法運(yùn)算，得到的新的高斯分布的方差比兩個(gè)做乘法的高斯分布都小。

兩個(gè)不那么確定的分布，最終得到了一個(gè)相對(duì)確定的分布，這是卡爾曼濾波的一直被推崇的原因。

再預(yù)測(cè)，再觀測(cè)（Prediction & Measurement）

第1秒的初始化以及第2秒的預(yù)測(cè)、觀測(cè)，實(shí)現(xiàn)卡爾曼濾波的一個(gè)周期。

同樣的，我們根據(jù)第2秒的狀態(tài)向量做第3秒的預(yù)測(cè)，再與第3秒的觀測(cè)結(jié)果進(jìn)行加權(quán)，就得到了第3秒的狀態(tài)向量；再根據(jù)第3秒的狀態(tài)向量做第4秒的預(yù)測(cè)，再與第4秒的觀測(cè)結(jié)果進(jìn)行加權(quán)，就得到了第4秒的狀態(tài)向量。以此往復(fù)，就實(shí)現(xiàn)了一個(gè)真正意義上的卡爾曼濾波器。

以上就是卡爾曼濾波器的感性分析過(guò)程，下面我們回歸理性，談?wù)勅绾螌⒁陨线^(guò)程寫(xiě)成代碼。