卡爾曼濾波器算法

本文購買自 https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc颈娜，修改了公式亂碼屠升，僅供學(xué)習(xí)，請支持原版購買

文章正文

首先聲明一下，此文是預(yù)備給初學(xué)者閱讀的缨称，旨在幫助初學(xué)者更好地理解卡爾曼濾波器算法，降低公式推導(dǎo)的難度祝迂，所以我盡量避免一些太過于學(xué)術(shù)化的講解睦尽。

我在學(xué)習(xí)卡爾曼濾波器算法時，也花了不少時間型雳。

有時候覺得自己弄懂了当凡，遇到新的問題時山害，還是不知道靈活運用，然后就發(fā)現(xiàn)自己還是沒有弄懂沿量，自己只是因為照著別人的博客順著別人的思路順利閱讀完畢而已浪慌，脫離了博客中的示例場景，切換到新的場景朴则，仍然不能很好地運用权纤，反反復(fù)復(fù)之后，某天閉眼也能手推卡爾曼濾波器涉及到的 5 個公式時佛掖，仿佛有大徹大悟的感覺妖碉，然后，我才認(rèn)為自己再遇到新的問題時芥被，我不會再像之前那樣不知從何入手了欧宜。

好了，正文開始拴魄。
先看看為什么叫“卡爾曼”冗茸。跟其他著名的理論（例如傅立葉變換，泰勒級數(shù)等等）一樣匹中，卡爾曼也是一個人的名字夏漱，而跟他們不同的是，他是個現(xiàn)代人顶捷！

卡爾曼全名 Rudolf Emil Kalman

匈牙利數(shù)學(xué)家挂绰，1930 年出生于匈牙利首都布達佩斯。1953服赎，1954 年于麻省理工學(xué)院分別獲得電機工程學(xué)士及碩士學(xué)位葵蒂。1957 年于哥倫比亞大學(xué)獲得博士學(xué)位。我們現(xiàn)在要學(xué)習(xí)的卡爾曼濾波器重虑，正是源于他的博士論文和 1960 年發(fā)表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預(yù)測問題的新方法）践付。

簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”缺厉。對于解決很大部分的問題永高，他是最優(yōu)，效率最高甚至是最有用的提针。他的廣泛應(yīng)用已經(jīng)超過 30 年命爬，包括機器人導(dǎo)航，控制辐脖，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導(dǎo)彈追蹤等等遇骑。近年來更被應(yīng)用于計算機圖像處理，例如頭臉識別揖曾，圖像分割落萎，圖像邊緣檢測等等亥啦。

1. 什么是卡爾曼濾波器？

卡爾曼濾波器是一種估計算法练链，在上個世紀(jì)六十年代就大顯神威了翔脱，NASA 將它應(yīng)用在阿波羅飛船的軌跡預(yù)測上面，所以它稱得上是一個能上天的算法媒鼓。

卡爾曼濾波器能夠?qū)ξ磥頂?shù)據(jù)做預(yù)測届吁，到今天，它在許多行業(yè)都發(fā)揮了重大的作用绿鸣，如導(dǎo)航疚沐、自動駕駛、電池電量預(yù)測潮模、雷達目標(biāo)追蹤亮蛔、機器視覺等等。

你可以將卡爾曼濾波器算法應(yīng)用在你想要預(yù)測的領(lǐng)域擎厢，例如市場上豬肉的價格究流、房間里面的溫度、太陽底下你影子的長度變化动遭、你個人網(wǎng)站或者是博客或者是 QQ 空間的訪問量等芬探，這個是靈活的，只要你愿意嘗試厘惦，反正它是一個優(yōu)秀的估計算法偷仿，耗費的硬件資源也少，靈活方便宵蕉，每一個程序員都應(yīng)該了解一下酝静。

2. 如何通俗解釋卡爾曼濾波器？

我們已經(jīng)知道国裳，卡爾曼濾波器算法可以做估算形入。這個小節(jié)全跨，我進一步舉例闡述缝左，讓大家對它的邏輯有一個進一步的感性認(rèn)知提神。

首先浓若，我們要明確地記得卡爾曼是用來估算的渺杉，這是我們使用的目的，很多人被復(fù)雜的公式推導(dǎo)搞不清頭腦挪钓，最終忘記了卡爾曼濾波器算法的本來目的是越，從而讓自己特別糾結(jié)躊躇。

其次碌上，如果我們能夠有辦法確定每時每刻的事件倚评，那么就不需要運用卡爾曼濾波器浦徊。卡爾曼濾波器算法適用于不能保證能夠準(zhǔn)確確認(rèn)的事件天梧。

簡單來講盔性，卡爾曼濾波就是加權(quán)，給不同的數(shù)據(jù)分配不同的權(quán)重呢岗，然后產(chǎn)生新的數(shù)據(jù)冕香，也是數(shù)據(jù)融合的意思。

比如后豫，公司叫你和小李同時評價新員工的表現(xiàn)悉尾。

你說這人太不靠譜了，滿分為 10 分的情況下最多打 6 分挫酿，小李卻說人不錯构眯，可以給 8 分。

那上司怎么辦呢饭豹？一般情況下最簡單的方法是求平均值鸵赖，也就是 6+8 等于 14，平均分是 7拄衰。

但一般靠譜的領(lǐng)導(dǎo)會有類似的邏輯處理事情它褪。

在領(lǐng)導(dǎo)眼中，你和小李說話的可信度是不一樣的翘悉。所以他可能信你少一些茫打，也可能信你多一點，所以妖混，最終的結(jié)果就不會是加權(quán)平均老赤。可能是 6*0.3+8*0.7=7.4制市。

這等效于這樣的公式：

最優(yōu)數(shù)據(jù) = 你的數(shù)據(jù) + K(小李的數(shù)據(jù) ? 你的數(shù)據(jù))
卡爾曼濾波器也正式類似的邏輯抬旺，不過做了些許巧妙的處理，首先它會計算你們的誤差祥楣，在這里是 8 分減去 6 分开财，為 2 分，接著用一個比例 K误褪，取值 0~1责鳍，最終結(jié)果是兩者都有考慮，但是權(quán)重不一致兽间。

$6+K?2$

如果 K 為 0历葛，則上司完全信任你曹洽，你說多少分就是多少分半等，如果 K 為 1 的話扁掸，那么就是 8 分坡氯，代表上司完全信任小李的評價。其它的情況咒程，K 在 0 和 1 之間问裕，最終的結(jié)果是你們的分?jǐn)?shù)綜合，但是權(quán)重不一樣孵坚。

K 等于 0.5 時粮宛，兩者權(quán)重一樣。

K 小于 0.5 時卖宠，偏向你巍杈，反之偏向小李。

在卡爾曼濾波器中 K 被稱為卡爾曼增益扛伍。

到這里筷畦，我們基本上知道了卡爾曼濾波器大概是怎么一回事了，下面我們還需要理解 3 個概念才算更進一步的理解刺洒。

預(yù)測（predict）
測量（mesasure）
估算（estimate）

我打個比方鳖宾。

王老板是做肉類批發(fā)生意的，他手下有員工小 A 和小 B逆航。

小 A 負責(zé)每天早上給王老板預(yù)測當(dāng)天肉類的價格鼎文。

小 B 每周一上午 9 點到專門的行政機構(gòu)去摘抄數(shù)據(jù)，然后匯報給王老板因俐。

所以拇惋，王老板有 2 個數(shù)據(jù)源，王老板懂得卡爾曼濾波器算法抹剩，所以撑帖，他能夠搞定價格估算這件事情。

小 A 和小 B 的數(shù)據(jù)來源不一樣澳眷，誤差也不一樣胡嘿。

小 A 的數(shù)據(jù)是靠經(jīng)驗拍腦袋決定的，被稱為預(yù)測（predict）钳踊。

小 B 的數(shù)據(jù)來源于機構(gòu)的統(tǒng)計衷敌，準(zhǔn)確度比較高，被稱為測量（measure）箍土，也被稱為校正（correct）逢享。

有同學(xué)可能會有疑問罐监，小 B 的數(shù)據(jù)很準(zhǔn)確吴藻，那么我們?yōu)槭裁床恢挥眯?B 的數(shù)據(jù)呢？

關(guān)鍵是弓柱，條件不允許啊沟堡，現(xiàn)實生活中侧但，很多的東西都是被制約的，比如航罗，小 B 只能在周一上午能夠得到數(shù)據(jù)禀横，但是其它 6 天，王老板也得做生意啊粥血，因此柏锄，王老板必須依賴于小 A 的經(jīng)驗預(yù)測。

但是為了防止這種拍腦袋的數(shù)據(jù)結(jié)果偏差太大复亏，所以王老板才會在適當(dāng)?shù)臅r候引入小 B 的數(shù)據(jù)趾娃，來校正小 A 的預(yù)測，最終王老板綜合兩者缔御，得到了自己的估算數(shù)據(jù)抬闷，并且將它作為每天的結(jié)果。

這個例子耕突，就是典型的卡爾曼濾波器算法所要涉及的基本概念示例笤成。

到這一步，我們已經(jīng)很清楚知道了這個算法的概念眷茁，包括預(yù)測炕泳、測量、估算上祈，也知道需要用一個卡爾曼增益來加權(quán)調(diào)和 2 個數(shù)據(jù)形成最終的估算數(shù)據(jù)喊崖。

如果是文科生，或者其他理工人員雇逞，了解了這么多后荤懂，已經(jīng)可以和人家談笑風(fēng)生講這個神奇的算法了。

但是塘砸，作為一個相關(guān)的軟件工程師节仿，這個還不夠的。

你還需要知道掉蔬，如何套用公式去求解：

預(yù)測的數(shù)據(jù)值 ,公式中用 $\hat{}$ 表示
測量的數(shù)據(jù)值廊宪，公式中用 $\bar{}$ 表示
卡爾曼增益，公式中用 K 表示
最終估算的數(shù)據(jù)值女轿，公式中用 $\tilde{}$ 表示

如果箭启，你是一個算法工程師，光知道用還是不夠的蛉迹，你得清楚知道公式怎么來的傅寡，透徹理解它的公式后，遇到新的應(yīng)用場景，你才能從容不迫荐操。

不然芜抒，總是反反復(fù)復(fù)陷入一種似懂非懂的困境。

3. 卡爾曼濾波器相關(guān)的公式

卡爾曼濾波器算法是時域上的線性變換估算托启。

在一個系統(tǒng)中宅倒，卡爾曼濾波器用狀態(tài)(state）來描述某個時刻的系統(tǒng)，它是一個向量屯耸。

比如拐迁，我要用一個向量來描述我每天的健康狀態(tài)，我選用了最低心率疗绣、最低血壓唠亚、疲勞度 3 個指標(biāo)。

$x=(68,88,20)^T$

這個數(shù)據(jù)怎么來的呢持痰？

我可以每周去醫(yī)院測量一次灶搜，可以準(zhǔn)確獲得。

其它時間工窍，是我自己的預(yù)測割卖，雖然和真實值有出入，但大致還過的過去患雏。

最關(guān)鍵的是鹏溯，我今天的預(yù)測和昨天相關(guān)，明天的和今天的相關(guān)淹仑，因為狀態(tài)是向量丙挽，所以很容易用線性代數(shù)的方式描述。

$x_{t} = F*x_{t-1} + \omega_{t}$

t 表示當(dāng)前時刻匀借，t-1 表示上一時刻颜阐，ω 表示此次預(yù)測的噪聲，假定 ω 符合高斯分布吓肋。
F 是狀態(tài)轉(zhuǎn)移矩陣凳怨，因為在線性代數(shù)中，矩陣表示變換是鬼，所以一個向量與一個矩陣相乘肤舞，一般會得到一個新的向量。
F 的值與具體的問題相關(guān)均蜜，實際解決問題時李剖，要分析狀態(tài)到狀態(tài)變換時的關(guān)系。

比如囤耳，我認(rèn)為我每天的狀態(tài)相差不大篙顺。所以偶芍，我的 F 可以是單位矩陣。

$\left\{ \begin{matrix} 1 & 0 & 0 \\ 0& 1 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right\}$
狀態(tài)的誤差可以設(shè)置為

$ω=(1,1,4)^T$

實際上慰安，完整的狀態(tài)變換公式應(yīng)該是：

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$

B 是控制矩陣，u 是控制變量聪铺，它們的乘積代表外力干預(yù)化焕。
- 比如，如果讓我加班铃剔，我的疲勞度會加 20撒桨，如果多放一天假，我的疲勞度會減少 15键兜。
- B 和 U 的取值同樣需要具體問題具體分析凤类。

用 $P_{t}$ 表示 $x_{t}$ 的協(xié)方差，有下面的公式普气。

$P_{t} = F*P_{t-1}*F^{T} +Q_{t}$

$Q_t$ 是噪聲的協(xié)方差谜疤。

這是預(yù)測相關(guān)的兩個公式，下面講測量现诀。

用 $Z_t$ 表示測量的數(shù)據(jù)夷磕。

$Z_{t} = H*S_{t} + \vec{u_{t}}$

H 是狀態(tài)變換矩陣，有時候測量的數(shù)據(jù)和預(yù)測的數(shù)據(jù)單位不統(tǒng)一仔沿，這個時候就需要轉(zhuǎn)換坐桩。
- 舉個例子，假設(shè)讓你預(yù)測今天的蘋果價格封锉，你說 10 元/斤绵跷，然后，網(wǎng)絡(luò)上的報價是 3.2$/kg成福，這就是單位不一致碾局，需要用 H 這樣的矩陣變換過來，才能有效處理奴艾。

測量的數(shù)據(jù)也需要記錄協(xié)方差信息擦俐，用 $\bar{P_t}$ 表示，有下面的公式：

$\bar{P_{t}} = H*\hat{P_{t}}*H^{T}+R_{t}.$

Rt 是測量噪聲 $\vec{v}$ 的協(xié)方差握侧。

有了預(yù)測和測量的公式蚯瞧，我們還需要估算的公式，在這之前我們知道品擎，我們要求 K,也就是卡爾曼增益埋合。

$K = P_{t}*H^{T}(H_{k}P_{t}H^{T}+R_{t})^{-1}$

最終，我們用 $\tilde{x_t}$ 表示經(jīng)卡爾曼濾波計算后的估計狀態(tài)萄传。

$\tilde{x_{t}} = \hat{x_{t}} + K * (z_{t} - H*\hat{lx_{t}})$

它也有協(xié)方差甚颂，用 $\hat{P_t}$ 表示：

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$

與卡爾曼濾波器相關(guān)的公式就全部羅列出來了蜜猾，一字排開如下：

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$ ............ (公式1)

$\hat{P_{t}} = F*\hat{P_{t-1}}*F^{T} +Q_{t}$ ................（公式 2）

$Z_{t} = H*x_{t} + \vec{\nu_{t}}$ ....................（公式 3）

$\bar{P_{t}} = H*\hat{P_{t}}*H^{T}+R_{t}$ ...........（公式 4）

$\tilde{x_{t}} = \hat{x_{t}} + K * (z_{t} - H*\hat{x_{t}})$ .......（公式 5）

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$ ............（公式 6）

$K = \hat{P_{t}}*H^{T}(H_{k}\hat{P_{t}}H^{T}+R_{t})^{-1}$ ....（公式 7）

有了上面的公式，只要我們好好調(diào)試預(yù)測誤差和測量誤差振诬，我們就能套用卡爾曼濾波器算法來取得比較好的效果蹭睡，對于一般工程師而言，這已經(jīng)足夠了赶么。

但是肩豁，如果你是算法崗位的話，這個還不夠的辫呻，你需要確定下面一些問題：

公式 2 怎么由公式 1 推導(dǎo)出來清钥？
公式 4 怎么由公式 3 推導(dǎo)出來？
公式 5 怎么推導(dǎo)出來的放闺？

4. 公式推導(dǎo)

本節(jié)的目的是便于理解為主祟昭，所以整個推導(dǎo)可能不會顯得那么正式與嚴(yán)肅。

首先怖侦，定義好符號篡悟。

因為涉及到預(yù)測值、測量值匾寝、估計值恰力，為了避免混淆，用不同的符號表示旗吁。

我們預(yù)測的狀態(tài)用 $\hat{x}$ 表示踩萎，其實也可以理解為先驗狀態(tài)。

用 $x$ 表示理論上的絕對真值很钓。

用 $\tilde{x}$ 表示經(jīng)卡爾曼濾波求出來的估算值香府，也稱為最優(yōu)值，也可以理解為后驗狀態(tài)码倦。

在這里插入圖片描述

無論是預(yù)測值企孩，還是估算值，與真實值之間還是存在誤差的袁稽。

在這里插入圖片描述

如上圖虛線表示勿璃，分別用 $\hat{e}$ 和 $\tilde{e}$ 表示。

接下來推汽，我們用一個協(xié)方差矩陣 $P_t$ 來表示預(yù)測值偏離真實值的狀態(tài)补疑。

根據(jù)協(xié)方差的公式可得，當(dāng)前 t 時刻的狀態(tài)協(xié)方差為：

$P_{t} = E[(\hat{e_{t}})(\hat{e_{t}})^T] = E[(x_{t}-\hat{x_{t}})(x_{t}-\hat{x_{t}})^T]$

因為卡爾曼濾波器是依照時間遞推的歹撒，所以 t-1 時刻的狀態(tài)協(xié)方差為 $P_{t?1}$ 莲组。

$P_{t-1} = E[(\hat{e_{t-1}})(\hat{e_{t-1}})^T] = E[(x_{t-1}-\hat{x_{t-1}})(x_{t-1}-\hat{x_{t-1}})^T]$

我們前面已經(jīng)知道，t 時刻的狀態(tài)可以由 t-1 時刻的狀態(tài)轉(zhuǎn)移過來,也就是如公式 1 所示暖夭。

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$ .........(公式1)

那么按照剛剛協(xié)方差的公式：

$P_{t} = E[(\hat{e_{t}})(\hat{e_{t}})^T] =$
$E[(F*x_{t-1}+B\vec{u_{t}}+\omega_{t}-F*\hat{x_{t-1}}-B\vec{u_{t}})(F*x_{t-1}+B\vec{u_{t}}+\omega_{t}-F*\hat{x_{t-1}}-B\vec{u_{t}})^T]$
$= E[(F(x_{t-1}-\hat{x_{t-1}})+\omega_{t})(F(x_{t-1}-\hat{x_{t-1}})+\omega_{t})^T]$

因為 x 和 ω 沒有什么相關(guān)性锹杈，根據(jù)概率論知識撵孤，兩個獨立的變量，協(xié)方差為 0竭望，所以上面的公式可以化簡為：

$=F*E[(x_{t-1}-\hat{x_{t-1}})(x_{t-1}-\hat{x_{t-1}})^T]*F^T+E[\omega_{t}\omega_{t}^T]$

仔細觀察公式邪码，可以看出來，公式實際上就是：

$P_{t}=F*P_{t-1}*F^T+Q_{t}.$

這實際上就是公式 2咬清，到這里闭专，公式 2 已經(jīng)就可以證明了。

神奇的是枫振， $B\vec{u}$ 的協(xié)方差沒有參與到迭代當(dāng)中喻圃。

實際上萤彩，概率論中有這么一個公式粪滤。

$Cov(Ax)=E(Ax?μ)(Ax?μ)^T)=A?Cov(x)?A$
那么通過這個公式也能直接又公式 1 推導(dǎo)出公式 2。

現(xiàn)在看公式 3雀扶。

$Z_{t} = H*x_{t} + \vec{\nu_{t}}$ ...........（公式 3）

我們依照同樣的方式可以推出公式 4杖小，我們有 $\tilde{P_t}$ 表示測量值的協(xié)方差，很容易得到：

$\tilde{P_{t}}=H*\tilde{P_{t}}*H^T+R_{t}$

公式 4 也證明了愚墓， $R_t$ 是測量誤差 $v$ 的協(xié)方差予权。

最難的是公式 5 的證明。

文章前面的小節(jié)我們已經(jīng)有了卡爾曼增益的概率浪册，也大概知道了怎么基于 2 個數(shù)據(jù)求最優(yōu)的估計扫腺。

現(xiàn)在，預(yù)測值是 $\hat{x}$ 村象，測量值是 $z$ 笆环，我們用 $\tilde{x}$ 來表示經(jīng)卡爾曼濾波估算的值。

在這里插入圖片描述

K 代表一種比例厚者，因為有 K 的存在躁劣，所以 $\tilde{x}$ 會在 $\hat{x}$ 和 $z$ 之間取值擺動：

$\tilde{x_{t}}=\hat{x_{t}}+K(z_{t}-\hat{x_{t}})$

根據(jù)前面測量的公式，可得：

$\tilde{x_{t}}=\hat{x_{t}}+K(H*x_{t}+v_{t}-H*\hat{x_{t}})$

展開這個公式库菲，可得：

$\tilde{x_{t}}=\hat{x_{t}}+kH*x_{t}+Kv_{t}-KH*\hat{x_{t}}$

接下來账忘，就有一個巧妙的變換：

$\tilde{x_{t}} - x =\hat{x_{t}}-x+kH*x_{t}-KH*\hat{x_{t}} + Kv_{t}$

實際上就是：

$\tilde{e_{t}}=(I-KH)\hat{e_{t}}-Kv_{t}$

巧妙的是到這一步，估計值的誤差和預(yù)測值的誤差還有測量的噪聲有了聯(lián)系熙宇。

我們用 $\tilde{P_t}$ 表示估計值的協(xié)方差鳖擒，根據(jù)協(xié)方差定義可得。

$\tilde{P_{t}} = E[\tilde{e_{t}}\tilde{e_{t}}^T]=(I-KH)\hat{P_{t}}(I-KH)^T+KR_{t}K^T$

現(xiàn)在烫止，只剩下 K 的值沒有確定了败去。

協(xié)方差代表偏離真實值的程度，卡爾曼濾波器的算法就是以不斷減少估計值對應(yīng)的協(xié)方差烈拒，從而讓估計值漸漸偏向于真實值圆裕。

所以广鳍，我們以 $\tilde{P_t}$ 建立一個目標(biāo)函數(shù)，目的就是求它最小值時吓妆，K 的取值赊时。

我們可以求 K 相對于 $\tilde{P}$ 的偏導(dǎo)數(shù),當(dāng)結(jié)果為 0 時，表示 K 是最佳的取值行拢。

可以得到：

$-2(\hat{P_{t}}H^T)+2K(H\hat{P_{t}}H^T+R_{t}) = 0$

可以得到：

$K = \hat{P_{t}}H^T(H\hat{P_{t}}H^T+R_{t})^{-1}$

K 求出來后祖秒，把它的值套入前面求出來的公式當(dāng)中， $\tilde{x_{t}}$ 和 $\tilde{P_{t}}$ 也就出來了舟奠。

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$ ........... (公式6)

5. 卡爾曼濾波器算法預(yù)測鼠標(biāo)指針坐標(biāo)

這個是比較經(jīng)典的一個卡爾曼濾波的示例竭缝。創(chuàng)建一個黑色背景的窗口，鼠標(biāo)移動時實時顯示它的位置沼瘫，這樣就形成了軌跡抬纸。

另外還將經(jīng)過卡爾曼濾波器算法估計的位置也顯示出來。

源代碼是 C++ 編寫的耿戚，軟件環(huán)境為 ubuntu16.04湿故、OpenCV3.3.0、CMake膜蛔。

我假設(shè)讀者都有 OpenCV 和 C++ 的基礎(chǔ)坛猪。

#include <opencv2/opencv.hpp>
#include <iostream>

cv::KalmanFilter kalmanfilter(2,2);
cv::Mat last_measurement(2,1,CV_32FC1);
cv::Mat current_measurement(2,1,CV_32FC1);

cv::Mat last_prediction(2,1,CV_32FC1);
cv::Mat current_prediction(2,1,CV_32FC1);

cv::Mat frame(800,800,CV_8UC3);

void onMouseMove(int event,int x,int y,int flag,void* data)
{

    last_prediction = current_prediction;
    last_measurement = current_measurement;

    current_measurement.at<float>(0) = x;
    current_measurement.at<float>(1) = y;

    std::cout << current_measurement << std::endl;

    kalmanfilter.correct(current_measurement);

    current_prediction = kalmanfilter.predict();
    //kalmanfilter.predict();

    cv::line(frame,cv::Point(last_measurement.at<float>(0),last_measurement.at<float>(1)),cv::Point(current_measurement.at<float>(0),current_measurement.at<float>(1)),cv::Scalar(0,255,0),2);
    cv::line(frame,cv::Point(last_prediction.at<float>(0),last_prediction.at<float>(1)),cv::Point(current_prediction.at<float>(0),current_prediction.at<float>(1)),cv::Scalar(0,0,255),2);

    std::cout << "on mouse move:" << current_prediction <<std::endl;

}

int main(int argc,char** argv)
{
    //cv::Mat frame(800,800,CV_8UC3);

    cv::namedWindow("test");
    cv::setMouseCallback("test",onMouseMove,NULL);

    cv::Mat F(2,2,CV_32F,cv::Scalar(0));
    //m.at<float>(0,0) = 1;
    //m.at<float>(1,1) = 1;

    cv::setIdentity(F,cv::Scalar(1));

    cv::Mat H(2,2,CV_32F);
    cv::setIdentity(H,cv::Scalar(1));

    cv::Mat p(2,2,CV_32F);
    cv::setIdentity(p,cv::Scalar(0.1));

    kalmanfilter.measurementMatrix = H;
    kalmanfilter.transitionMatrix = F;
    kalmanfilter.processNoiseCov = p;

    while (true)
    {
        cv::imshow("test",frame);

        if (cv::waitKey(30) == 113)
            break;
    }

    cv::destroyAllWindows();

    return 0;
}

源碼采用 CMake 編譯。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(test)

find_package(OpenCV)

include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(test main.cpp)
target_link_libraries(test ${OpenCV_LIBS})

在當(dāng)前目錄創(chuàng)建一個 build 目錄皂股。然后編譯執(zhí)行墅茉。

mkdir build

cd build

cmake ..

make

./test

最終結(jié)果，如下圖：

kalman_mouse

紅色的軌跡是預(yù)測的呜呐。

綠色的軌跡是測量得到的就斤。

為了示例的簡單化，我將測量矩陣和過程矩陣都設(shè)置為了單位矩陣卵史，過程噪聲也設(shè)置得比較少战转，測量噪聲就干脆沒有設(shè)置。大家可以自行調(diào)整參數(shù)以躯。

大家注意看動畫中鼠標(biāo)急劇轉(zhuǎn)彎時槐秧，綠色的軌跡馬上能夠反映出來，而紅色的軌跡則比較圓潤忧设，這也體現(xiàn)了卡爾曼濾波的平滑效果刁标。

6. 卡爾曼濾波器算法在自動駕駛當(dāng)中的應(yīng)用示例

前面說過，卡爾曼濾波器的算法在很多行業(yè)都有廣泛的應(yīng)用址晕，只要你熟練掌握了它膀懈，那么它就能夠為你所用。

現(xiàn)在谨垃，很多高配的車上都配備了 ADAS 功能启搂，能夠有效地檢測道路線和其他車輛硼控，從而潛在危險發(fā)生時，車輛會預(yù)警胳赌。

那么牢撼，檢測車輛就成為一項必須完成的硬指標(biāo)。

能夠識別車輛的算法有很多疑苫，有傳統(tǒng)的機器視覺算法熏版，也有神經(jīng)網(wǎng)絡(luò)。

目前捍掺，威力最大的還是 SSD 和 YOLO v3撼短，尤其是 YOLO v3，檢測速度非惩ξ穑快曲横。

但是，有一個問題是满钟，在汽車上軟件都要運行在嵌入式平臺胜榔，硬件資源有限胳喷，所以車輛檢測耗時要比 PC 上要久湃番。

這涉及到一個實時性的問題，安裝在車輛上的攝像頭吭露，每秒要處理 24 幀才算實時吠撮，也就是要在 50 毫秒內(nèi)完成所有的圖像處理代碼，很可惜的是這很難辦到讲竿。

所以泥兰，需要卡爾曼濾波器這樣的算法來估計。

假設(shè)算法檢測到一輛車需要 200ms题禀，卡爾曼濾波器預(yù)估算法只要 3ms鞋诗，那么我們可以每隔 4 幀用算法檢測一次車輛位置，而其它時間我們用卡爾曼濾波器算法估計車輛位置迈嘹。

算法檢測出來的車輛可以當(dāng)做是測量數(shù)據(jù)削彬，其余的就是卡爾曼濾波器的預(yù)估，最終平均下來每一幀的處理時間不超過 50ms秀仲，也就達到了實時性的要求融痛。

在這里插入圖片描述

這個是 YOLO 算法檢測出來的，不一定每一幀都可以檢測的到神僵，我們可以看到 bbox 在閃爍雁刷，抖動比較大。

在這里插入圖片描述

這個是 YOLO 結(jié)合卡爾曼濾波器做出來的效果保礼，可以看到白色的框抖動比較小沛励，起到了平滑作用责语。

7. 總結(jié)

總體上來講，卡爾曼濾波器要理解起來并不難目派。

但是要靈活運用去解決新的問題鹦筹，而不僅僅停留在別人的示例 Demo 中的話，需要自己下一番功夫址貌，要好好理解它铐拐。

現(xiàn)在，將容易混淆的地方再羅列一下练对。

預(yù)測值（predict）遍蟋，這是先驗概率，是靠系統(tǒng)的轉(zhuǎn)移矩陣得到的值螟凭，誤差比較大虚青。
測量值（measure），誤差較小螺男，但也存在噪聲棒厘。
估計值（estimate），經(jīng)測量值修正的的一個值下隧，也就是后驗概率奢人。

觀察各自的協(xié)方差，可以發(fā)現(xiàn)每次預(yù)測的時候淆院，過去的誤差會累計到當(dāng)前何乎，按照這個趨勢，預(yù)測值會離真實值越來越遠土辩。

在這里插入圖片描述

而測量值的介入支救，經(jīng)過卡爾曼增益的比例調(diào)和，新的估計值會往真實值回歸拷淘，協(xié)方差公式也說明了各墨，它是遞減的。

在這里插入圖片描述

上圖中启涯，圓圈代表測量階段的測量值贬堵，它的介入會產(chǎn)生一個估計值，也就是藍色的向量逝嚎，這讓原本的軌跡向理論上的真實值靠攏扁瓢，經(jīng)過不停的迭代，整個系統(tǒng)預(yù)測的軌跡也會越來越準(zhǔn)补君，這就是卡爾曼濾波器的美妙之處引几。

卡爾曼公式求證的手段和角度有很多，但本質(zhì)上它和最小二乘法相差不大，不同之處是它是迭代遞歸的伟桅。

如果敞掘，有同學(xué)對向量的協(xié)方差不熟悉，可以推薦《程序員的數(shù)學(xué) 2 概率統(tǒng)計》這本書楣铁，如果對于矩陣求導(dǎo)不熟悉玖雁，網(wǎng)絡(luò)上有大量的資料可以查閱。

最后盖腕，本文介紹的是最基本的卡爾曼濾波赫冬，它適合線性的系統(tǒng)，但現(xiàn)實生活中溃列，很多系統(tǒng)是非線性的劲厌，解決這類問題可以用 EKF 和 UKF 兩種，有興趣的同學(xué)可以自行深入了解听隐。

本文首發(fā)于 GitChat补鼻，https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc