卡爾曼濾波器的理論

前文用了一個(gè)簡(jiǎn)答的例子對(duì)卡爾曼濾波器的整個(gè)流程進(jìn)行了介紹，下面我們根據(jù)卡爾曼濾波器的原理，編寫(xiě)代碼，跟蹤連續(xù)的激光雷達(dá)點(diǎn)。

在這里就要祭出卡爾曼老先生給我們留下的寶貴財(cái)富了，下面7個(gè)公式就是卡爾曼濾波器的理性描述，使用下面7個(gè)公式，就能夠?qū)崿F(xiàn)一個(gè)完整的卡爾曼濾波器?，F(xiàn)在看不懂這7個(gè)公式?jīng)]關(guān)系，繼續(xù)往下看，我會(huì)一個(gè)一個(gè)做解釋。

寫(xiě)代碼（C++）的過(guò)程，實(shí)際上就是結(jié)合上面的公式，一步步完成初始化、預(yù)測(cè)、觀測(cè)的過(guò)程。由于公式中涉及大量的矩陣轉(zhuǎn)置和求逆運(yùn)算，我們使用開(kāi)源的矩陣運(yùn)算庫(kù)——Eigen庫(kù)。

代碼：初始化（Initialization）

在Initialization這一步，需要將各個(gè)變量初始化，對(duì)于不同的運(yùn)動(dòng)模型，其狀態(tài)向量肯定是不一樣的，比如前文小車的例子，只需要一個(gè)距離s和一個(gè)速度v就可以表示小車的狀態(tài)；再比如在一個(gè)2維空間中的點(diǎn)，需要x方向上的距離和速度以及y方向上的距離和速度才能表示，這樣的狀態(tài)方程就有4個(gè)變量。

因此我們使用Eigen庫(kù)中非定長(zhǎng)的數(shù)據(jù)結(jié)構(gòu)，下圖中的VerctorXd表示X維的列矩陣，其中的元素?cái)?shù)據(jù)類型為double。

在這里，我們新建了一個(gè)KalmanFilter類，其中定義了一個(gè)叫做x_的變量，表示這個(gè)卡爾曼濾波器的狀態(tài)向量。

代碼：預(yù)測(cè)（Prediction）

完成初始化后，我們開(kāi)始寫(xiě)Prediction部分的代碼。首先是公式

這里的x為狀態(tài)向量，通過(guò)左乘一個(gè)矩陣F，再加上外部的影響u，得到預(yù)測(cè)的狀態(tài)向量x'。這里的F成為狀態(tài)轉(zhuǎn)移矩陣（state transistion matrix）。以2維的勻速運(yùn)動(dòng)為例，這里的x為

對(duì)于勻速運(yùn)動(dòng)模型，根據(jù)中學(xué)物理課本中的公式s1 = s0 + vt，經(jīng)過(guò)時(shí)間△t后的預(yù)測(cè)狀態(tài)向量應(yīng)該是

由于假設(shè)當(dāng)前運(yùn)動(dòng)為勻速運(yùn)動(dòng)，加速度為0，加速度不會(huì)對(duì)預(yù)測(cè)造成影響，即

如果換成加速或減速運(yùn)動(dòng)模型，就可以引入加速度a_x和a_y，根據(jù)s1 = s0 + vt + at^2/2這里的u會(huì)變成：

作為入門課程，這里不討論太復(fù)雜的模型，因此公式

最終將寫(xiě)成

由于每次做預(yù)測(cè)時(shí)，△t的大小不固定，因此我們專門寫(xiě)一個(gè)函數(shù)SetF()。

再看預(yù)測(cè)模塊的第二個(gè)公式

該公式中P表示系統(tǒng)的不確定程度，這個(gè)不確定程度，在卡爾曼濾波器初始化時(shí)會(huì)很大，隨著越來(lái)越多的數(shù)據(jù)注入濾波器中，不確定程度會(huì)變小，P的專業(yè)術(shù)語(yǔ)叫狀態(tài)協(xié)方差矩陣（state covariance matrix）；這里的Q表示過(guò)程噪聲（process covariance matrix），即無(wú)法用x'=Fx+u表示的噪聲，比如車輛運(yùn)動(dòng)時(shí)突然到了上坡，這個(gè)影響是無(wú)法用之前的狀態(tài)轉(zhuǎn)移估計(jì)的。

以激光雷達(dá)為例。激光雷達(dá)只能測(cè)量點(diǎn)的位置，無(wú)法測(cè)量點(diǎn)的速度，因此對(duì)于激光雷達(dá)的協(xié)方差矩陣來(lái)說(shuō)，對(duì)于位置信息，其測(cè)量位置較準(zhǔn)，不確定度較低；對(duì)于速度信息，不確定度較高。因此可以認(rèn)為這里的P為：

由于Q對(duì)整個(gè)系統(tǒng)存在影響，但又不能太確定對(duì)系統(tǒng)的影響有多大。工程上，我們一般將Q設(shè)置為單位矩陣參與運(yùn)算，即

根據(jù)以上內(nèi)容和公式

我們就可以寫(xiě)出預(yù)測(cè)模塊的代碼了

際編程時(shí)x'及P'不需要申請(qǐng)新的內(nèi)存去存儲(chǔ)，使用原有的x和P代替即可。

代碼：觀測(cè)（Measurement）

觀測(cè)的第一個(gè)公式是

這個(gè)公式計(jì)算的是實(shí)際觀測(cè)到的測(cè)量值z(mì)與預(yù)測(cè)值x'之間差值y。不同傳感器的測(cè)量值一般不同，比如激光雷達(dá)測(cè)量的位置信號(hào)為x方向和y方向上的距離，毫米波雷達(dá)測(cè)量的是位置和角度信息。因此需要將狀態(tài)向量左乘一個(gè)矩陣H，才能與測(cè)量值進(jìn)行相應(yīng)的運(yùn)算，這個(gè)H被稱為測(cè)量矩陣（Measurement Matrix）。

激光雷達(dá)的測(cè)量值為

其中xm和ym分別表示x方向上的測(cè)量（measurement）值。

由于x'是一個(gè)4*1的列向量，如果要與一個(gè)2*1的列向量z進(jìn)行減運(yùn)算，需要左乘一個(gè)2*4的矩陣才行，因此整個(gè)公式最終要寫(xiě)成：

即，對(duì)于激光雷達(dá)來(lái)說(shuō)，這里的測(cè)量矩陣H為：

求得y值后，對(duì)y值乘以一個(gè)加權(quán)量，再加到原來(lái)的預(yù)測(cè)量上去，就可以得到一個(gè)既考慮了測(cè)量值，又考慮了預(yù)測(cè)模型的位置的狀態(tài)向量了。

那么y的這個(gè)權(quán)值該如何取呢？

再看接下里的兩個(gè)公式

這兩個(gè)公式求的是卡爾曼濾波器中一個(gè)很重要的量——卡爾曼增益K（Kalman Gain），用人話講就是求y值的權(quán)值。

公式中的R是測(cè)量噪聲矩陣（measurement covariance matrix），這個(gè)表示的是測(cè)量值與真值之間的差值。一般情況下，傳感器的廠家會(huì)提供該值。S只是為了簡(jiǎn)化公式，寫(xiě)的一個(gè)臨時(shí)變量，不要太在意。

看最后兩個(gè)公式

這兩個(gè)公式，實(shí)際上完成了卡爾曼濾波器的閉環(huán)，第一個(gè)公式是完成了當(dāng)前狀態(tài)向量x的更新，不僅考慮了上一時(shí)刻的預(yù)測(cè)值，也考慮了測(cè)量值，和整個(gè)系統(tǒng)的噪聲，第二個(gè)公式根據(jù)卡爾曼增益，更新了系統(tǒng)的不確定度P，用于下一個(gè)周期的運(yùn)算，該公式中的I為與狀態(tài)向量同維度的單位矩陣。