道不盡的卡爾曼！通俗易懂詳細(xì)解釋卡爾曼濾波

概述

總的來(lái)說(shuō)，卡爾曼濾波器是一個(gè)狀態(tài)估計(jì)器，它利用傳感器融合、信息融合來(lái)提高系統(tǒng)的精度。通常，我們要觀測(cè)一個(gè)系統(tǒng)的狀態(tài)，有兩種手段。一種是通過(guò)系統(tǒng)的狀態(tài)轉(zhuǎn)移方程，并結(jié)合上一時(shí)刻的狀態(tài)推得下一時(shí)刻的狀態(tài)。一種是借助輔助系統(tǒng)（量測(cè)系統(tǒng)）的測(cè)量得到系統(tǒng)狀態(tài)。這兩種方式都有各自的不確定性，卡爾曼濾波可以將這兩者做到最優(yōu)結(jié)合（加權(quán)平均），使得我們估計(jì)的狀態(tài)的不確定性小于其中任何一種。所以權(quán)重的選擇至關(guān)重要，它意味著我們更信任哪一種方式得出的狀態(tài)（當(dāng)然是更加信任不確定性較小的狀態(tài)）。

建模

比如，我們觀測(cè)一輛小車的速度和位置（所以我們要觀察的狀態(tài)就是速度 v 和位置 p ，即狀態(tài) x=(v,p) ,這里的 p,v 都是向量）,當(dāng)我們有了 k-1 時(shí)刻的狀態(tài) =( , ) 后，那么如果速度不變，我們就可以得到下一時(shí)刻的狀態(tài) =( , ) ,其中

當(dāng)然，我們也可以對(duì)小車進(jìn)行控制，比如讓小車加速、拐彎等等，這些都可以用一個(gè)加速度來(lái)表示： ，這樣，下一時(shí)刻狀態(tài) =( , ) 的表示變?yōu)椋?/p>

其中：

但是，現(xiàn)實(shí)的情況并沒(méi)有那么理想，小車可能會(huì)受到外界的各種擾動(dòng)，比如，輪胎打滑，地面崎嶇等等，都會(huì)使得狀態(tài)轉(zhuǎn)移的過(guò)程中混入噪音干擾（過(guò)程噪聲），而且這個(gè)噪音往往是不可被測(cè)量的，也就是說(shuō)，沒(méi)辦法通過(guò)建模消除噪音項(xiàng)。不過(guò)，往往我們可以認(rèn)為這個(gè)噪音是服從零均值的高斯分布的。那么（1）式重寫為

上面的狀態(tài)轉(zhuǎn)移方程為我們提供了觀察系統(tǒng)狀態(tài)的一種方式，同時(shí)，我們可能有一些輔助系統(tǒng)，比如對(duì)于這個(gè)小車，我們可以用GPS作為得到其狀態(tài)的另一種方式。

那樣的話，GPS每一時(shí)刻都可以提供一次當(dāng)前狀態(tài)的觀測(cè)值 ,它與真實(shí)狀態(tài) 的關(guān)系為， = +  （3）

其中的 是觀測(cè)模型，它把系統(tǒng)真實(shí)狀態(tài)空間映射成觀測(cè)空間， 是噪聲項(xiàng)，稱為觀測(cè)噪聲——因?yàn)槿魏蔚臏y(cè)量系統(tǒng)都是有誤差的，所以觀測(cè)值實(shí)際上是真實(shí)值與噪聲的疊加。我們同樣可以認(rèn)為此噪聲是服從0均值的高斯分布的。即

現(xiàn)在我們比較這兩種方式得到的系統(tǒng)狀態(tài)，容易想到，狀態(tài)轉(zhuǎn)移方程得到的系統(tǒng)狀態(tài)在演變時(shí)會(huì)非常平滑，而它的不確定度會(huì)隨著迭代的進(jìn)行而逐漸增大，因?yàn)檎`差會(huì)在一次次迭代的過(guò)程中不斷累積（具體反映為估計(jì)狀態(tài)的方差越來(lái)越大）。相反，由量測(cè)系統(tǒng)得到的狀態(tài)不存在累積誤差，但演變時(shí)也會(huì)很不平滑。這時(shí)我們就需要將兩者得到的狀態(tài)有效結(jié)合起來(lái)。這就是卡爾曼濾波做的事情了。

卡爾曼濾波

然后我們要算出測(cè)量殘差：

第一項(xiàng)為0，因?yàn)? 代表k時(shí)刻系統(tǒng)狀態(tài)的真實(shí)值，是一個(gè)固定值，沒(méi)有協(xié)方差。所以簡(jiǎn)化為：

補(bǔ)充內(nèi)容：

另外，直觀上理解：協(xié)方差矩陣代表的是數(shù)據(jù)分布的一種不確定度，這種不確定度本身不具有所謂的正負(fù)號(hào)，不可能因?yàn)榧訙p而有所抵消。