詳解卡爾曼濾波（Kalman Filter）原理

前言

看過(guò)很多關(guān)于卡爾曼濾波的資料哲银，發(fā)現(xiàn)很多資料寫(xiě)的都很晦澀，初學(xué)者很難看懂。在網(wǎng)上找了很多資料之后词顾，發(fā)現(xiàn)了這篇博文講的非常清晰易懂炸裆，特此翻譯記錄垃它，以備后用。為保證翻譯的原滋原味烹看，以下均用第一人稱(chēng)表述国拇。

背景

我不得不說(shuō)一說(shuō)卡爾曼濾波，因?yàn)樗茏龅降氖虑楹?jiǎn)直令人驚嘆惯殊。

很可惜的是酱吝，很少有軟件工程師和科學(xué)家對(duì)此有深入了解。這讓我感到很沮喪土思，因?yàn)榭柭鼮V波是如此通用且強(qiáng)大的工具务热，它能在不確定情況下組合信息。有時(shí)己儒，它提取準(zhǔn)確信息的能力似乎幾乎是不可思議的崎岂。如果聽(tīng)起來(lái)我講的太多了，那么請(qǐng)看一下之前發(fā)布的視頻闪湾，其中演示了一個(gè)利用卡爾曼濾波觀察自由浮動(dòng)物體的速度來(lái)確定它的方向冲甘。真棒！

卡爾曼濾波是什么途样？

你可以在任何含有不確定信息的動(dòng)態(tài)系統(tǒng)中的使用卡爾曼濾波江醇，對(duì)系統(tǒng)的下一步動(dòng)作做出有根據(jù)的猜測(cè)。即使伴隨著各種干擾何暇，卡爾曼濾波總是能指出真實(shí)世界發(fā)生的情況嫁审。它可以利用怪異現(xiàn)象之間的關(guān)聯(lián)，你可能不會(huì)想到利用這些關(guān)聯(lián)赖晶！

卡爾曼濾波對(duì)于持續(xù)變化的系統(tǒng)是理想的選擇律适。由于卡爾曼濾波除了記憶前一個(gè)狀態(tài)而不需要保留其他的歷史記憶信息，因此卡爾曼濾波具有輕量化的特點(diǎn)遏插，運(yùn)行速度非澄婊撸快，非常適合處理實(shí)時(shí)的問(wèn)題和嵌入式系統(tǒng)胳嘲。

你在Google上找到的大部分關(guān)于卡爾曼濾波的數(shù)學(xué)描述是晦澀難懂的闪萄。那是非常糟糕的狀況襟锐！因?yàn)榭柭鼮V波能被簡(jiǎn)單和容易的方式所理解的间影。因此，本文是一個(gè)非常不錯(cuò)的文章主題辰妙，本文將嘗試用許多清晰、美觀的圖片來(lái)闡明它甫窟。本文的前提很簡(jiǎn)單密浑，你僅僅需要對(duì)概率和矩陣有基本的了解。

本文將從一個(gè)簡(jiǎn)單的例子開(kāi)始粗井，說(shuō)明卡爾曼濾波可以解決的問(wèn)題尔破。但如果你想直接接觸精美的圖片和數(shù)學(xué)，請(qǐng)隨時(shí)跳轉(zhuǎn)浇衬。

我們能用卡爾曼濾波做什么懒构？

舉一個(gè)簡(jiǎn)單的小例子：你已經(jīng)做了一個(gè)能在叢林中游走的小機(jī)器人，為確保其能導(dǎo)航耘擂，機(jī)器人需要知道它所在的位置胆剧。

將機(jī)器人的運(yùn)動(dòng)狀態(tài)表示為

\vec{x_k}

，其僅僅包含了位置和速度：

\vec{x_k}=(\vec{p},\vec{v})

請(qǐng)注意醉冤，狀態(tài)只是關(guān)于系統(tǒng)底層配置的數(shù)字列表秩霍，它可以是任何東西。在我們的例子中冤灾，它是位置和速度，但它可以是油箱中液體量的數(shù)據(jù)辕近、汽車(chē)引擎的溫度韵吨、用戶手指在觸控板上的位置或者任何你需要跟蹤的東西。

我們的機(jī)器人也有GPS傳感器移宅，精確大約10米归粉，但它需要更精確地知道自己的位置。在樹(shù)林中有很多溝壑和懸崖漏峰，如果機(jī)器人的誤差超過(guò)幾英尺糠悼，它可能會(huì)從懸崖上掉下去。所以僅依賴GPS進(jìn)行定位是遠(yuǎn)遠(yuǎn)不夠的浅乔。

我們可能還知道機(jī)器人是如何移動(dòng)的：機(jī)器人知道發(fā)送給車(chē)輪馬達(dá)的指令倔喂，如果它朝一個(gè)方向前進(jìn)，沒(méi)有任何干擾靖苇，下一刻它可能會(huì)繼續(xù)朝同一方向前進(jìn)席噩。當(dāng)然，它對(duì)自己的運(yùn)動(dòng)并不完全了解：它可能會(huì)受到風(fēng)的沖擊贤壁，車(chē)輪可能會(huì)打滑悼枢，或者在崎嶇不平的地形上滾動(dòng)；所以輪子轉(zhuǎn)動(dòng)的數(shù)量可能不能準(zhǔn)確地代表機(jī)器人實(shí)際行走了多遠(yuǎn)脾拆，這個(gè)預(yù)測(cè)也不會(huì)完全準(zhǔn)確馒索。

GPS傳感器告訴我們一些關(guān)于狀態(tài)的信息莹妒，但只是間接的，帶有一些不確定性而且并不精準(zhǔn)绰上。我們的預(yù)測(cè)告訴了機(jī)器人是如何移動(dòng)的旨怠，但只是間接的，并且也是不確定和不精準(zhǔn)的渔期。

但是运吓，如果我們利用所有可用的信息，我們能得到一個(gè)比這兩個(gè)估計(jì)本身更好的答案嗎?當(dāng)然疯趟，答案是肯定的拘哨，這就是卡爾曼濾波器的作用。

如何從卡爾曼濾波的角度看待問(wèn)題信峻？

讓我們來(lái)看看我們想要詮釋的一個(gè)場(chǎng)景倦青。我們繼續(xù)上一個(gè)例子，機(jī)器人僅僅包含一個(gè)位置和速度的簡(jiǎn)單狀態(tài)盹舞。

\vec{x}=\begin{bmatrix} p \\ v \end{bmatrix}

我們不知道實(shí)際的位置和速度是什么产镐；有一系列可能的位置和速度組合可能是正確的，但其中一些狀態(tài)比其他的狀態(tài)更可能：

卡爾曼濾波器假設(shè)兩個(gè)變量（例子中為位置和速度）都是隨機(jī)變量且服從高斯分布的踢步。隨機(jī)變量的均值為

\mu

癣亚，它表示隨機(jī)分布的中心位置，即機(jī)器人最可能的狀態(tài)获印，不確定性用方差

\sigma^2

表示：

在上圖中述雾，位置和速度是不相關(guān)的，這意味著一個(gè)變量的狀態(tài)不能推測(cè)出其他變量的狀態(tài)兼丰。

更有趣的是下面的例子：位置和速度是呈相關(guān)性的玻孟。觀察特定位置的可能性取決于你的速度：

這種情況可能會(huì)出現(xiàn)。例如：根據(jù)一個(gè)舊的位置估計(jì)一個(gè)新的位置鳍征，如果速度很快黍翎，可能會(huì)走得更遠(yuǎn)，所以位置會(huì)更遠(yuǎn)艳丛。如果走得很慢匣掸，那么就不會(huì)走的很遠(yuǎn)。

這種關(guān)系非常重要氮双，因?yàn)樗o我們提供了更多的信息：一個(gè)測(cè)量值告訴我們其他測(cè)量值可能是什么旺聚。我們要盡可能多地從不確定的目標(biāo)中壓縮卡爾曼濾波器的信息！

這種相關(guān)性被稱(chēng)為協(xié)方差矩陣眶蕉。簡(jiǎn)而言之砰粹，矩陣的每個(gè)元素

\sum\nolimits_{ij}

是第

i

個(gè)狀態(tài)變量和

j

個(gè)狀態(tài)變量之間的相關(guān)程度。（由于協(xié)方差矩陣是對(duì)稱(chēng)的，也就是交換

i

和

j

不會(huì)影響最終的結(jié)果)碱璃。協(xié)方差矩陣往往用“

\sum

”來(lái)表示弄痹，其中的元素則表示為“

\sum\nolimits_{ij}

”。

矩陣化描述問(wèn)題

我們基于高斯分布來(lái)建立狀態(tài)變量嵌器，所以在時(shí)間

k

需要兩條信息：將最佳估計(jì)稱(chēng)為

\hat{\mathbf{x}}_{\mathbf{k}}

（即均值

\mu

）和它的協(xié)方差矩陣

\rm{P_k}

肛真。

\hat{\mathbf{x}}_{\mathbf{k}} = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} \\ \rm{P_k} = \begin{bmatrix} \sum\nolimits_{pp} & \sum\nolimits_{pv} \\ \sum\nolimits_{vp} & \sum\nolimits_{vv} \end{bmatrix} \tag{1}

(在這里本文只使用了位置和速度，但是該狀態(tài)可以包含任意數(shù)量的變量爽航，并表示任何需要表示的東西蚓让，這對(duì)于處理其他問(wèn)題是非常有益的)。

接下來(lái)讥珍，我們需要某種方式來(lái)知道目前狀態(tài)（時(shí)刻

k-1

）并預(yù)測(cè)下一個(gè)時(shí)刻

k

的狀態(tài)历极。記住，我們不知道哪個(gè)狀態(tài)是“真實(shí)的”狀態(tài)衷佃，但我們的預(yù)測(cè)函數(shù)并不關(guān)心趟卸。它適用于所有的情況，并給出了一個(gè)新的分布：

用矩陣

\mathbf{F_k}

表示這個(gè)預(yù)測(cè)步驟：

它將我們?cè)脊烙?jì)中的每個(gè)點(diǎn)都移動(dòng)到了一個(gè)新的預(yù)測(cè)位置氏义，如果原始估計(jì)是正確的話锄列，這個(gè)新的預(yù)測(cè)位置就是系統(tǒng)下一步會(huì)移動(dòng)到的位置。那我們又如何用矩陣來(lái)預(yù)測(cè)下一個(gè)時(shí)刻的位置和速度呢惯悠？下面用一個(gè)基本的運(yùn)動(dòng)學(xué)公式來(lái)表示：

\begin{aligned} \hat{\mathbf{x}}_{k} &=\left[\begin{array}{cc} 1 & \Delta t \\ 0 & 1 \end{array}\right] \hat{\mathbf{x}}_{k-1} \\ &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} \end{aligned}

\begin{aligned} \operatorname{Cov}(x) &=\Sigma \\ \operatorname{Cov}(\mathbf{A} x) &=\mathbf{A} \Sigma \mathbf{A}^{T} \end{aligned}

\begin{array}{l} \hat{\mathbf{x}}_{k}=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} \\ \mathbf{P}_{k}=\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} \end{array}

外部控制變量

\begin{equation} \begin{array}{l} p_{k}=p_{k-1}+\Delta t v_{k-1}+\frac{1}{2} a \Delta t^{2} \\ v_{k}=\quad v_{k-1}+a \Delta t \end{array} \end{equation}

\begin{equation} \begin{aligned} \hat{\mathbf{x}}_{k} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\left[\begin{array}{c} \frac{\Delta t^{2}}{2} \\ \Delta t \end{array}\right] a \\ &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}} \end{aligned} \end{equation}

外部干擾

\begin{aligned} \hat{\mathbf{x}}_{k} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}} \\ \mathbf{P}_{k} &=\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T}+\mathbf{Q}_{k} \end{aligned}

利用外部觀測(cè)值修正估計(jì)量

\begin{array}{l} \vec{\mu}_{\text {expected }}=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} \\ \mathbf{\Sigma}_{\text {expected }}=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} \end{array}

聯(lián)合高斯分布

\mathcal{N}(x, \mu, \sigma)=\frac{1}{\sigma \sqrt{2 \pi}} e^{-\frac{(x-\mu)^{2}}{2 \sigma^{2}}}

\mathcal{N}\left(x, \mu_{0}, \sigma_{0}\right) \cdot \mathcal{N}\left(x, \mu_{1}, \sigma_{1}\right) \stackrel{?}{=} \mathcal{N}\left(x, \mu^{\prime}, \sigma^{\prime}\right)

\begin{equation} \begin{aligned} \mu^{\prime} &=\mu_{0}+\frac{\sigma_{0}^{2}\left(\mu_{1}-\mu_{0}\right)}{\sigma_{0}^{2}+\sigma_{1}^{2}} \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\frac{\sigma_{0}^{4}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned} \end{equation}

\mathbf{k}=\frac{\sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}}

\begin{aligned} \mu^{\prime} &=\mu_{0}+\mathbf{k}\left(\mu_{1}-\mu_{0}\right) \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\mathbf{k} \sigma_{0}^{2} \end{aligned}

\mathbf{K}=\Sigma_{0}\left(\Sigma_{0}+\Sigma_{1}\right)^{-1}

\begin{array}{l} \vec{\mu}^{\prime}=\overrightarrow{\mu_{0}}+\mathbf{K}\left(\overrightarrow{\mu_{1}}-\overrightarrow{\mu_{0}}\right) \\ \Sigma^{\prime}=\Sigma_{0}-\mathbf{K} \Sigma_{0} \end{array}

整合上述公式

\begin{aligned} \mathbf{H}_{k} \hat{\mathbf{x}}_{k}^{\prime} &=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} &+\mathbf{K}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{H}_{k} \mathbf{P}_{k}^{\prime} \mathbf{H}_{k}^{T} &=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} &-\mathbf{K} \mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} \end{aligned}

\mathbf{K}=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1}

\begin{aligned} \hat{\mathbf{x}}_{k}^{\prime} &=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{P}_{k}^{\prime} &=\mathbf{P}_{k^{-}} \mathbf{K}^{\prime} \mathbf{H}_{k} \mathbf{P}_{k} \end{aligned}

\mathbf{K}^{\prime}=\mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1}

總結(jié)

對(duì)于上述所有的數(shù)學(xué)公式邻邮，你僅僅需要實(shí)現(xiàn)公式（7）、（18）和（19）克婶。（如果你忘記了上述公式筒严，你也能從公式（4）和（5）重新推導(dǎo)。）

這將允許你精確地建模任何線性系統(tǒng)鸠补。對(duì)于非線性系統(tǒng)萝风，需要用到擴(kuò)展卡爾曼濾波嘀掸，區(qū)別在于EKF多了一個(gè)把預(yù)測(cè)和測(cè)量部分進(jìn)行線性化的過(guò)程紫岩。

最后編輯于：2020.10.04 09:29:20

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末，一起剝皮案震驚了整個(gè)濱河市睬塌，隨后出現(xiàn)的幾起案子泉蝌，更是在濱河造成了極大的恐慌，老刑警劉巖揩晴，帶你破解...
沈念sama閱讀 219,427評(píng)論 6贊 508
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件勋陪，死亡現(xiàn)場(chǎng)離奇詭異，居然都是意外死亡硫兰，警方通過(guò)查閱死者的電腦和手機(jī)诅愚，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 93,551評(píng)論 3贊 395
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門(mén)，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)劫映，“玉大人违孝，你說(shuō)我怎么就攤上這事刹前。” “怎么了雌桑？”我有些...
開(kāi)封第一講書(shū)人閱讀 165,747評(píng)論 0贊 356
道士緝兇錄：失蹤的賣(mài)姜人
文/不壞的土叔我叫張陵喇喉，是天一觀的道長(zhǎng)。經(jīng)常有香客問(wèn)我校坑，道長(zhǎng)拣技，這世上最難降的妖魔是什么？我笑而不...
開(kāi)封第一講書(shū)人閱讀 58,939評(píng)論 1贊 295
?港島之戀（遺憾婚禮）
正文為了忘掉前任耍目，我火速辦了婚禮膏斤，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘制妄。我一直安慰自己掸绞，他們只是感情好，可當(dāng)我...
茶點(diǎn)故事閱讀 67,955評(píng)論 6贊 392
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開(kāi)白布耕捞。她就那樣靜靜地躺著衔掸，像睡著了一般。火紅的嫁衣襯著肌膚如雪俺抽。梳的紋絲不亂的頭發(fā)上敞映，一...
開(kāi)封第一講書(shū)人閱讀 51,737評(píng)論 1贊 305
城市分裂傳說(shuō)
那天，我揣著相機(jī)與錄音磷斧，去河邊找鬼振愿。笑死，一個(gè)胖子當(dāng)著我的面吹牛弛饭，可吹牛的內(nèi)容都是我干的冕末。我是一名探鬼主播，決...
沈念sama閱讀 40,448評(píng)論 3贊 420
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開(kāi)眼侣颂，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼档桃！你這毒婦竟也來(lái)了？” 一聲冷哼從身側(cè)響起憔晒，我...
開(kāi)封第一講書(shū)人閱讀 39,352評(píng)論 0贊 276
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤藻肄，失蹤者是張志新（化名）和其女友劉穎，沒(méi)想到半個(gè)月后拒担，有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體嘹屯，經(jīng)...
沈念sama閱讀 45,834評(píng)論 1贊 317
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 37,992評(píng)論 3贊 338
?白月光啟示錄
正文我和宋清朗相戀三年从撼，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了州弟。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片。...
茶點(diǎn)故事閱讀 40,133評(píng)論 1贊 351
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖婆翔，靈堂內(nèi)的尸體忽然破棺而出桐经，到底是詐尸還是另有隱情，我是刑警寧澤浙滤，帶...
沈念sama閱讀 35,815評(píng)論 5贊 346
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布阴挣，位于F島的核電站，受9級(jí)特大地震影響纺腊，放射性物質(zhì)發(fā)生泄漏畔咧。R本人自食惡果不足惜，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 41,477評(píng)論 3贊 331
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一揖膜、第九天我趴在偏房一處隱蔽的房頂上張望誓沸。院中可真熱鬧，春花似錦壹粟、人聲如沸拜隧。這莊子的主人今日做“春日...
開(kāi)封第一講書(shū)人閱讀 32,022評(píng)論 0贊 22
一樁弒父案趁仙，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)洪添。三九已至，卻和暖如春雀费，著一層夾襖步出監(jiān)牢的瞬間干奢，已是汗流浹背。一陣腳步聲響...
開(kāi)封第一講書(shū)人閱讀 33,147評(píng)論 1贊 272
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工盏袄，沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留忿峻，地道東北人。一個(gè)月前我還...
沈念sama閱讀 48,398評(píng)論 3贊 373
代替公主和親
正文我出身青樓辕羽，卻偏偏與公主長(zhǎng)得像逛尚，于是被迫代替她去往敵國(guó)和親。傳聞我的和親對(duì)象是個(gè)殘疾皇子刁愿，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 45,077評(píng)論 2贊 355