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

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

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

初始化（Initialization）

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

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

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

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

預(yù)測（Prediction）

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

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

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

觀測（Measurement）

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

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

圖中紅色區(qū)域為預(yù)測的小車位置，藍(lán)色區(qū)域為第2秒的觀測結(jié)果。

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

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

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

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

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

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

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

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

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

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