▌導(dǎo)論 卡爾曼濾波本質(zhì)上是一個數(shù)據(jù)融合算法,，將具有同樣測量目的、來自不同傳感器,、(可能) 具有不同單位 (unit) 的數(shù)據(jù)融合在一起,，得到一個更精確的目的測量值。 卡爾曼濾波的局限性在于其只能擬合線性高斯系統(tǒng),。但其最大的優(yōu)點在于計算量小,，能夠利用前一時刻的狀態(tài)（和可能的測量值）來得到當(dāng)前時刻下的狀態(tài)的最優(yōu)估計。 本文雖然是小白教程,，但還是需要各位至少知道高斯分布,，一點點線性代數(shù)，還有狀態(tài)向量這樣的名詞,。 簡述考慮一個SLAM 問題,，它由一個運動方程： 和一個觀測方程組成： 就把它當(dāng)作一個線性系統(tǒng)吧（非線性系統(tǒng)請看下一講擴(kuò)展卡爾曼濾波），并且為了簡化推導(dǎo),，忽略路標(biāo)的下標(biāo)j,，并把路標(biāo)y 并入到狀態(tài)向量一起優(yōu)化，那么運動方程就可以寫為： 其中：
這一步在卡爾曼濾波中也稱為預(yù)測 (predict)。 類似,，測量方程可以寫為：
其中:
而卡爾曼濾波就是預(yù)測 - 測量之間不斷循環(huán)迭代,。當(dāng)然,，對于某些情況，如GPS + IMU,，由于IMU 測量頻率遠(yuǎn)比GPS 高,，在只有IMU 測量值時，只執(zhí)行運動更新,，在有GPS 測量值時再進(jìn)行測量更新,。 ▌一個小例子 用一個在解釋卡爾曼濾波時最常用的一維例子：小車追蹤。如下圖所示：
狀態(tài)向量為小車的位置和速度：
而司機要是踩了剎車或者油門,，小車就會具有一個加速度,，
。 假設(shè)t 和t-1 時刻之間的時間差為
,。根據(jù)物理知識,，有：
寫成矩陣形式就有：
跟之前的運動方程對比，就知道
上式就寫為：
表示t-1 時刻卡爾曼濾波的狀態(tài)估計,；
則表示中t-1 到t 時刻,，預(yù)測更新所得的預(yù)測值。 再利用運動模型對狀態(tài)向量進(jìn)行更新后,，還要繼續(xù)更新狀態(tài)向量的協(xié)方差矩陣P,，公式為：
假設(shè)
為t 時刻下狀態(tài)向量的真值（自然是永遠(yuǎn)未知的），由之前的現(xiàn)形運動方程（式(3)）給出,，將式(3) 與式(9) 相減可得：
而：
考慮到狀態(tài)向量和噪聲是不相關(guān)的,，則
，上式就可以簡化為：
推導(dǎo)完畢,。 可以看到，經(jīng)過預(yù)測更新,，協(xié)方差矩陣P 變大了,。這是因為狀態(tài)轉(zhuǎn)換并不完美，而且運動測量值含有噪聲,，具有較大的不確定性,。 預(yù)測更新實際上相當(dāng)于“加法”：將當(dāng)前狀態(tài)轉(zhuǎn)換到下一時刻（并增加一定不確定性），再把外界的干擾（運動測量值）疊加上去（又增加了一點不確定性）
上面即為卡爾曼濾波中預(yù)測這一步,。這一步相對比較直觀,，推導(dǎo)也較測量更新簡單，就只在這里詳細(xì)給出了,。 如果得到了測量值,，那么我們就可以對狀態(tài)向量進(jìn)行測量更新了，對應(yīng)的公式為：
其中：
為卡爾曼增益。 從這里就可以看到,，測量更新顯然比預(yù)測更新復(fù)雜,，難點也集中在這里。下面就給出測量更性的詳細(xì)推導(dǎo),。 ▌推導(dǎo) 一些CASE 從t-1 時刻起,，小車運動后，經(jīng)過前面所述的預(yù)測更新后,，我們就得到了t 時刻的小車位置的估計,，由于在卡爾曼濾波中，我們使用高斯概率分布來表示小車的位置,，因此這個預(yù)測的位置可以寫為：
為了與前面的通用的推導(dǎo)區(qū)別開來,，在這個一維的例子中我們使用了新的符號。不過熟悉高斯概率分布的話應(yīng)該可以馬上看出來,，
為這個高斯分布的均值,，
為方差，而r 為小車的可能位置,，
為某個可能位置 (r) 的概率分布,。 假設(shè)在t 時刻，我們通過某測距儀測得小車距離原點的距離r,，由于測量包含噪聲（且在面前我們假設(shè)了其為高斯噪聲）,，因此該測量值也可以利用高斯概率分布來表示：
除了下標(biāo)外，其余的字母的含義都和上面的式子一樣,。
如上圖瑣事,，現(xiàn)在在t 時刻，我們有了兩個關(guān)于小車位置的估計,。而我們所能得到的關(guān)于小測位置的最佳估計就是將預(yù)測更新和測量更新所得的數(shù)據(jù)融合起來,，得到一個新的估計。而這個融合,，就是一個簡單的“乘法”,，并利用了一個性質(zhì)：兩個高斯分布的乘積仍然是高斯分布。
將上式化簡一下：
其中,，
為
和
的加權(quán)平均,，
則是
和
的調(diào)和平均的二分一：
最右邊的式子是為了后面的計算而準(zhǔn)備的。 本質(zhì)上,，這（高斯分布相乘）就是卡爾曼濾波中測量更新的全部了,。 那么， 怎么由上面兩個簡單的一維的式子得到前一節(jié)
和
呢,？一步一步來,。 ▌轉(zhuǎn)換矩陣H的引入 在剛剛的一維情況的小例子中,，我們其實做了一個隱式的假設(shè)，即有預(yù)測更新得到的位置的概率分布和測距儀所得的測量值具有相同的單位 (unit),，如米 (m),。 但實際情況往往不是這樣的，比如,，測距儀給出的可能不是距離,，而是信號的飛行時間（由儀器至小車的光的傳播時間），單位為秒 (s),。這樣的話,，我們就無法直接如上面一般直接將兩個高斯分布相乘了。 此時,，就該轉(zhuǎn)換矩陣
閃亮登場了,。由于
，c 為光速,。所以此時
（測量方程為
,，可以回去參考一下式(4)）。 預(yù)測值就要寫為：
而測量值保持不變：
這樣,，兩個高斯概率分布在轉(zhuǎn)換矩陣H 的作用下又在同一個空間下了,。根據(jù)前面
和
的公式 （式(27) 和式(28)），可得：
將上式兩端都乘以c 則可得：
由于
（這里轉(zhuǎn)換矩陣H 不隨時間變化而變化,，所以把下標(biāo)t 略去）,，并記
，則上式可以寫為：
類似的有：
兩邊乘以
有：
▌推廣至高維 到了這一步,，這個一維情況下卡爾曼濾波的測量更新步驟就已經(jīng)徹底講完了,。剩下的就是將這個一維例子推廣至高維空間中。其實大家仔細(xì)觀察一下就會得到答案,。
最后,，根據(jù)式(33)
就可得：
根據(jù)
就可得：
▌小結(jié)一下 通過這個一維情況的推導(dǎo)，希望能說明卡爾曼濾波就是在給定初始值的情況下,，由預(yù)測和測量不斷迭代,、更新狀態(tài)向量。而預(yù)測就是一個“加法”：狀態(tài)轉(zhuǎn)換和運動預(yù)測疊加,；測量則是簡單的高斯分布相乘,，中間引入了一個轉(zhuǎn)換矩陣將測量值和狀態(tài)向量映射在同一個代數(shù)空間中。 ▌討論 至此,，相信你已經(jīng)明白了卡爾曼濾波的推導(dǎo)過程,。而具體的問題就取決于你的建模了。如在上面的小車的例子中,，各個參數(shù)如下：
假設(shè)該時刻下運動測量值為加速度
,，為均值為0標(biāo)準(zhǔn)差為
的高斯分布（標(biāo)準(zhǔn)差可以為經(jīng)驗值或儀器精度。
：
,、 對于測量值,，同樣可以假設(shè)
，那么
,。 ▌多問個為什么 如果只關(guān)心卡爾曼濾波的推導(dǎo)和結(jié)果,，到這里就可以停止啦。 但推完卡爾曼濾波,，我還有幾個個為什么,。知其然更要知其所以然。下面是我對于自己的疑惑學(xué)習(xí),、思考得到的解答,，而且礙于表達(dá)能力，不敢說百分百正確,。 首先是對于預(yù)測更新,。前面也說到了，預(yù)測更新相當(dāng)于“加法”,。這相對好理解一些,。在t-1 時刻我們有了對于小車位置的一個估計,，根據(jù)對小車速度（狀態(tài)向量之一）、小車的加速度（運動測量值）的建模,，在輔以時間間隔,，自然可以計算出小車在該時間間隔內(nèi)的位移和速度增量，再將之疊加到原有的狀態(tài)向量上即可,。由于建模和測量的過程帶有噪聲,，所以此時小車的位置估計的精度是下降的（方差增大）。 那么為什么測量更新就是乘法而非加法呢,？ 因為測量更新的依據(jù)是貝葉斯法則,。在有了測量值之后，我們求小車位置的概率分布其實就是在求
,。根據(jù)貝葉斯法則有：
是后驗概率,。直接求后驗概率比較困難（為什么？）,。假設(shè)就在這個一維的小車?yán)又?，?dāng)我們得到一個距離測量值z，那么小車的位置可能是距離原點的-z 或z 的兩個點上,。對于二維就可能是一個圓,，三維則是一個球。此時要精確地知道小車的位置（消除歧義點）,，一則我們可以繼續(xù)測量,，二則需要額外的信息。這就使得求后驗概率比較費時費力,。 反觀貝葉斯法則的右側(cè),，此時我們已經(jīng)有了先驗概率
，這是上一時刻的狀態(tài)向量的概率分布,，并且我們也有了
,，因為所得的測量值
表達(dá)的就是在當(dāng)前位置下，我們能得到的測量值,，亦即貝葉斯中的似然,。在
為一個常數(shù)的情況下，最大化
就得到了最優(yōu)的
,。 既然測量更新是以貝葉斯公式為基礎(chǔ),，那么反觀預(yù)測更新，除了前面那個直觀的“加法”解釋之外,，是不是也有一個概率上的解釋呢,？ 連續(xù)的高斯分布所表示的小車位置的預(yù)測更新我沒找到（不好意思），但就離散情況的話還是有的,，就是全概率公式,。以下圖為例，假設(shè)t-1 時刻,，小車的位置分布概率如圖所示,，到了t 時刻，預(yù)測小車向前運動了3米（3個格子）,，但由于模型的不確定性和噪音,，我們不能保證小車精確地向前走了3米，根據(jù)概率分布,，我們可以假設(shè)小車有80%的概率往前走了3米,，10%的概率往前走了兩米，而另有10%的概率往前走了4米,。
那么,，在t 時刻，若小車真的運動到了這個位置,，其概率分布是怎樣的呢,？它既有可能是在距離該位置3米遠(yuǎn)的地方以0.8的概率運動到現(xiàn)在這個位置的，也有可能是以0.1 的概率從2或4米遠(yuǎn)的地方為初始位置運動到這的,，根據(jù)全概率公式,，可以表達(dá)為：
類似地，t時刻下,，小車運動后在其他位置上的概率分布也可以用全概率公式表達(dá)出來,。當(dāng)然，最后的計算結(jié)果還需要進(jìn)行歸一化處理,。 如果我們不斷地減小每個方格的分辨率,，并按照高斯分布給予每個方格一個概率值，并對小車運動也做如此的離散化處理,，應(yīng)該也是可以不斷逼近連續(xù)的情況（個人猜想）,。 至此，關(guān)于卡爾曼濾波的個人淺見就到此為止了,。精通還需要不斷地實踐,，但希望讀完本文，能讓你對卡爾曼濾波有全面的了解,。 |
|
|
