↑ 點擊藍(lán)字?關(guān)注極市平臺

作者丨vincentqin

來源丨計算機視覺SLAM

編輯丨極市平臺

極市導(dǎo)讀

本文使用一系列圖示，清晰地介紹了卡爾曼濾波的背景和解決實際問題的過程。圖文并茂，值得一看。>>加入極市CV技術(shù)交流群，走在計算機視覺的最前沿

1 背景

關(guān)于濾波

首先援引來自知乎大神的解釋：

“一位專業(yè)課的教授給我們上課的時候，曾談到：filtering is weighting（濾波即加權(quán)）。濾波的作用就是給不同的信號分量不同的權(quán)重。最簡單的loss pass filter，就是直接把低頻的信號給1權(quán)重，而給高頻部分0權(quán)重。對于更復(fù)雜的濾波，比如維納濾波，則要根據(jù)信號的統(tǒng)計知識來設(shè)計權(quán)重。

從統(tǒng)計信號處理的角度，降噪可以看成濾波的一種。降噪的目的在于突出信號本身而抑制噪聲影響。從這個角度，降噪就是給信號一個高的權(quán)重而給噪聲一個低的權(quán)重。維納濾波就是一個典型的降噪濾波器。”

關(guān)于卡爾曼濾波

Kalman Filter 算法，是一種遞推預(yù)測濾波算法，算法中涉及到濾波，也涉及到對下一時刻數(shù)據(jù)的預(yù)測。Kalman Filter 由一系列遞歸數(shù)學(xué)公式描述。它提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方誤差最小?？柭鼮V波器應(yīng)用廣泛且功能強大：它可以估計信號的過去和當(dāng)前狀態(tài)，甚至能估計將來的狀態(tài)，即使并不知道模型的確切性質(zhì)。

Kalman Filter 也可以被認(rèn)為是一種數(shù)據(jù)融合算法（Data fusion algorithm），已有50多年的歷史，是當(dāng)今使用最重要和最常見的數(shù)據(jù)融合算法之一。Kalman Filter 的巨大成功歸功于其小的計算需求，優(yōu)雅的遞歸屬性以及作為具有高斯誤差統(tǒng)計的一維線性系統(tǒng)的最優(yōu)估計器的狀態(tài)。

Kalman Filter 只能減小均值為0的測量噪聲帶來的影響。只要噪聲期望為0，那么不管方差多大，只要迭代次數(shù)足夠多，那效果都很好。反之，噪聲期望不為0，那么估計值就還是與實際值有偏差[3]。

問題描述

上面的描述可能把大家繞暈了，下面來點輕松的。

我們會有一個疑問：卡爾曼濾波到底是如何解決實際問題的呢？

我們以機器人為例介紹卡爾曼濾波的原理，我們的任務(wù)是要預(yù)測機器人的狀態(tài)，包括位置與速度，即可表示為：

某個時刻，我們不知道真實的位置與速度到底是多少，二者存在一個所有可能性的組合，大致如下圖所示。

卡爾曼濾波假設(shè)狀態(tài)所有的變量都是隨機的且都服從高斯分布，每個變量都有其對應(yīng)的均值以及方差（它代表了不確定性）。

在上圖中，位置和速度是不相關(guān)（uncorrelated）的，這意味著某個變量的狀態(tài)不會告訴你其他變量的狀態(tài)是怎樣的。即，我們雖然知道現(xiàn)在的速度，但無法從現(xiàn)在的速度推測出現(xiàn)在的位置。但實際上并非如此，我們知道速度和位置是有關(guān)系的（correlated），這樣一來二者之間的組合關(guān)系變成了如下圖所示的情況。

這種情況是很容易發(fā)生的，例如，如果速度很快，我們可能會走得更遠(yuǎn)，所以我們的位置會更大。如果我們走得很慢，我們就不會走得太遠(yuǎn)。

這種狀態(tài)變量之間的關(guān)系很重要，因為它可以為我們提供更多信息：One measurement tells us something about what the others could be。這就是卡爾曼濾波器的目標(biāo)，我們希望從不確定的測量中盡可能多地獲取信息！

這種狀態(tài)量的相關(guān)性可以由協(xié)方差矩陣表示。簡而言之，矩陣的每個元素是第i個狀態(tài)變量和第j個狀態(tài)變量之間的相關(guān)度。（顯然地可以知道協(xié)方差矩陣是對稱的，這意味著交換i和j都沒關(guān)系）。協(xié)方差矩陣通常標(biāo)記為“?”，因此我們將它們的元素稱為“”。

2 狀態(tài)預(yù)測

問題的矩陣形式表示

我們把狀態(tài)建模成高斯分布（Gaussian blob，由于二維高斯分布長得像一個個小泡泡，之所以長這個樣子，可參考鏈接[2]）。我們需要求解/估計在時間時刻的兩個信息：1. 最優(yōu)估計以及它的協(xié)方差矩陣，我們可以寫成下面矩陣形式：

當(dāng)然，這里我們僅使用位置和速度，但是請記住狀態(tài)可以包含任意數(shù)量的變量，并且可以表示所需的任何變量。

接下來，我們需要某種方式來查看當(dāng)前狀態(tài)（時刻）并預(yù)測在時刻處的狀態(tài)。請記住，我們不知道哪個狀態(tài)是“真實”狀態(tài)，但是這里提到的預(yù)測（prediction）并不在乎這些。

我們可以用一個矩陣來表示這個預(yù)測過程：

這個矩陣將原始估計中的每個點移動到新的預(yù)測位置。

那么問題來了，應(yīng)該如何使用上述矩陣來預(yù)測下一時刻的位置和速度呢？為了闡述這個過程，我們使用了一個非?；A(chǔ)的運動學(xué)公式（初中物理中就學(xué)過）進(jìn)行描述：

寫成矩陣形式：

現(xiàn)在我們有了一個預(yù)測矩陣或者叫做狀態(tài)轉(zhuǎn)移矩陣，該矩陣可以幫助我們計算下一個時刻的狀態(tài)。但我們?nèi)匀徊恢廊绾胃聽顟B(tài)的協(xié)方差矩陣，其實過程也是很簡單，如果我們將分布中的每個點乘以矩陣，那么其協(xié)方差矩陣會發(fā)生什么？

將公式(3)代入公式(4)我們可以得到：

External influence

不過我們并沒有考慮到所有的影響因素?？赡苡幸恍┡c狀態(tài)本身無關(guān)的變化——如外界因素可能正在影響系統(tǒng)。

例如，我們用狀態(tài)對列車的運動進(jìn)行建模，如果列車長加大油門，火車就加速。同樣，在我們的機器人示例中，導(dǎo)航系統(tǒng)軟件可能會發(fā)出使車輪轉(zhuǎn)動或停止的命令。如果我們很明確地知道這些因素，我們可以將其放在一起構(gòu)成一個向量，我們可以對這個量進(jìn)行某些“處理”，然后將其添加到我們的預(yù)測中對狀態(tài)進(jìn)行更正。

假設(shè)我們知道由于油門設(shè)置或控制命令而產(chǎn)生的預(yù)期加速度。根據(jù)基本運動學(xué)原理，我們可以得到下式：

將其寫成矩陣形式：

其中被稱為控制矩陣，被稱為控制向量。（注意：對于沒有外部影響的簡單系統(tǒng)，可以忽略這個控制項）。

如果我們的預(yù)測并不是100％準(zhǔn)確模型，這會發(fā)生什么呢？

External uncertainty

如果狀態(tài)僅僅依賴其自身的屬性進(jìn)行演進(jìn)，那一切都會很好。如果狀態(tài)受到外部因素進(jìn)行演進(jìn)，我們只要知道這些外部因素是什么，那么一切仍然很好。

但在實際使用中，我們有時不知道的這些外部因素到底是如何被建模的。例如，我們要跟蹤四軸飛行器，它可能會隨風(fēng)搖晃；如果我們跟蹤的是輪式機器人，則車輪可能會打滑，或者因地面顛簸導(dǎo)致其減速。我們無法跟蹤這些外部因素，如果發(fā)生任何這些情況，我們的預(yù)測可能會出錯，因為我們并沒有考慮這些因素。

通過在每個預(yù)測步驟之后添加一些新的不確定性，我們可以對與“世界”相關(guān)的不確定性進(jìn)行建模（如我們無法跟蹤的事物）：

這樣一來，由于新增的不確定性原始估計中的每個狀態(tài)都可能遷移到多個狀態(tài)。因為我們非常喜歡用高斯分布進(jìn)行建模，此處也不例外。我們可以說的每個點都移動到具有協(xié)方差的高斯分布內(nèi)的某個位置，如下圖所示：

這將產(chǎn)生一個新的高斯分布，其協(xié)方差不同（但均值相同）：

所以呢，我們在狀態(tài)量的協(xié)方差中增加額外的協(xié)方差，所以預(yù)測階段完整的狀態(tài)轉(zhuǎn)移方程為：

換句話說：新的最佳估計是根據(jù)先前的最佳估計做出的預(yù)測，再加上對已知外部影響的校正。

新的不確定度是根據(jù)先前的不確定度做出的預(yù)測，再加上來自環(huán)境額外的不確定度。

上述過程描繪了狀態(tài)預(yù)測過程，那么當(dāng)我們從傳感器中獲取一些測量數(shù)據(jù)時會發(fā)生什么呢？

3 狀態(tài)更新

利用測量進(jìn)一步修正狀態(tài)

假設(shè)我們有幾個傳感器，這些傳感器可以向我們提供有關(guān)系統(tǒng)狀態(tài)的信息。就目前而言，測量什么量都無關(guān)緊要，也許一個讀取位置，另一個讀取速度。每個傳感器都告訴我們有關(guān)狀態(tài)的一些間接信息（換句話說，傳感器在狀態(tài)下運作并產(chǎn)生一組測量讀數(shù)）。

請注意，測量的單位可能與狀態(tài)量的單位不同。我們使用矩陣對傳感器的測量進(jìn)行建模。

所以我們期望傳感器的度數(shù)可以被建模成如下形式：

卡爾曼濾波器的偉大之處就在于它能夠處理傳感器噪聲。換句話說，傳感器本身的測量是不準(zhǔn)確的，且原始估計中的每個狀態(tài)都可能導(dǎo)致一定范圍的傳感器讀數(shù)，而卡爾曼濾波能夠在這些不確定性存在的情況下找到最優(yōu)的狀態(tài)。

根據(jù)傳感器的讀數(shù)，我們會猜測系統(tǒng)正處于某個特定狀態(tài)。但是由于不確定性的存在，某些狀態(tài)比其他狀態(tài)更可能產(chǎn)生我們看到的讀數(shù)：

我們將這種不確定性（如傳感器噪聲）的協(xié)方差表示為，讀數(shù)的分布均值等于我們觀察到傳感器的讀數(shù)，我們將其表示為

這樣一來，我們有了兩個高斯分布：一個圍繞通過狀態(tài)轉(zhuǎn)移預(yù)測的平均值，另一個圍繞實際傳感器讀數(shù)。

因此，我們需要將基于預(yù)測狀態(tài)（粉紅色）的推測讀數(shù)與基于實際觀察到的傳感器讀數(shù)（綠色）進(jìn)行融合。

那么融合后最有可能的新狀態(tài)是什么？對于任何可能的讀數(shù)，我們都有兩個相關(guān)的概率：（1）我們的傳感器讀數(shù)是的測量值的概率，以及（2）先前估計值的概率認(rèn)為是我們應(yīng)該看到的讀數(shù)。

如果我們有兩個概率，并且想知道兩個概率都為真的機會，則將它們相乘。因此，我們對兩個高斯分布進(jìn)行了相乘處理：

兩個概率分布相乘得到的就是上圖中的重疊部分。而且重疊部分的概率分布會比我們之前的任何一個估計值/讀數(shù)都精確得多，這個分布的均值就是兩種估計最有可能配置（得到的狀態(tài)）。

事實證明，兩個獨立的高斯分布相乘之后會得到一個新的具有其均值和協(xié)方差矩陣的高斯分布！下面開始推公式。

合并兩個高斯分布

首先考慮一維高斯情況：一個均值為，方差為的高斯分布的形式為：

我們想知道將兩個高斯曲線相乘會發(fā)生什么。下圖中的藍(lán)色曲線表示兩個高斯總體的（未歸一化）交集：

將公式(9)代入公式(10)，我們可以得到新的高斯分布的均值和方差如下所示：

我們將其中的一小部分重寫為：

這樣一來，公式的形式就簡單多了！我們順勢將公式(12)和(13)的矩陣形式寫在下面：

其中表示新高斯分布的協(xié)方差矩陣，是每個維度的均值，就是大名鼎鼎的“卡爾曼增益”（Kalman gain）。

公式匯總

我們有兩個高斯分布，一個是我們預(yù)測的觀測，另外一個是實際的觀測(傳感器讀數(shù))，我們將這兩個高斯分布代入公式(15)中就可以得到二者的重疊區(qū)域：

從公式(14)我們可以知道，卡爾曼增益是：

然后我們將公式(16)與公式(17)中的去除，同時將后面的去除，我們可以得到最終的化簡形式的更新方程：

4 圖說

大功告成，就是更新后的最優(yōu)狀態(tài)！接下來我們可以繼續(xù)進(jìn)行預(yù)測，然后更新，重復(fù)上述過程。下圖給出卡爾曼濾波信息流：

5 總結(jié)

在上述所有數(shù)學(xué)公式中，你需要實現(xiàn)的只是公式(7)(18)和(19)。

或者，如果你忘記了這些，可以從等式(4)和(15)重新推導(dǎo)所有內(nèi)容。

這將使你能夠準(zhǔn)確地對任何線性系統(tǒng)建模。對于非線性系統(tǒng)，我們使用擴(kuò)展卡爾曼濾波器，該濾波器通過簡單地線性化預(yù)測和測量值的均值進(jìn)行工作。

參考資料

[1]: How a Kalman filter works, in pictures, 圖解卡爾曼濾波是如何工作的：

?http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits

[2]: A geometric interpretation of the covariance matrix, 協(xié)方差矩陣的幾何解釋:?

https://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix

[3]: Kalman Filter 卡爾曼濾波:?

https://sikasjc.github.io/2018/05/08/kalman_filter

推薦閱讀

正式開賽！ACCV 2020國際細(xì)粒度網(wǎng)絡(luò)圖像識別競賽——是時候展現(xiàn)真正的技術(shù)了！
消除Aliasing！加州大學(xué)&英偉達(dá)提出深度學(xué)習(xí)下采樣新思路：自適應(yīng)低通濾波器層
即插即用的漲點神器，南航開源AFF：注意力特征融合

ACCV 2020國際細(xì)粒度網(wǎng)絡(luò)圖像識別競賽正式開賽！

添加極市小助手微信（ID : cvmart2），備注：姓名-學(xué)校/公司-研究方向-城市（如：小極-北大-目標(biāo)檢測-深圳），即可申請加入極市目標(biāo)檢測/圖像分割/工業(yè)檢測/人臉/醫(yī)學(xué)影像/3D/SLAM/自動駕駛/超分辨率/姿態(tài)估計/ReID/GAN/圖像增強/OCR/視頻理解等技術(shù)交流群：每月大咖直播分享、真實項目需求對接、求職內(nèi)推、算法競賽、干貨資訊匯總、與?10000+來自港科大、北大、清華、中科院、CMU、騰訊、百度等名校名企視覺開發(fā)者互動交流~

△長按添加極市小助手

△長按關(guān)注極市平臺，獲取最新CV干貨

覺得有用麻煩給個在看啦~??

一文圖解卡爾曼濾波(Kalman Filter)