最近業(yè)余在研究物體追蹤，看到傳統(tǒng)的方法用到了卡爾曼濾波（Kalman Filter）+匈牙利算法做軌跡匹配，因而開始研究這兩種算法是如何實(shí)現(xiàn)的。這里簡單總結(jié)一下卡爾曼濾波算法探索的過程。

▊ 卡爾曼濾波的背景

卡爾曼濾波常用于動(dòng)態(tài)多變化系統(tǒng)中的狀態(tài)估計(jì)，是一種通用性強(qiáng)的自回歸濾波器。它的由來和NASA登月有關(guān)。其發(fā)明者魯?shù)婪?E.卡爾曼在一次訪問NASA的時(shí)候，發(fā)現(xiàn)阿波羅計(jì)劃中一個(gè)難點(diǎn)是軌道預(yù)測問題，因而提出了一種濾波器，可以幫助高效預(yù)測軌跡，輔助導(dǎo)航。NASA最終使用了這個(gè)濾波器，然后成功實(shí)現(xiàn)人類第一次登月計(jì)劃。卡爾曼濾波器由此得名。

卡爾曼濾波器可以用來估計(jì)不確定信息，并給出狀態(tài)量下一時(shí)刻的情況。即便在有噪聲干擾的情況下，也可以較好的預(yù)測下一狀態(tài)的情況，并找出多變量間不易察覺的相關(guān)性。因而卡爾曼濾波器可以很好適應(yīng)不斷變化的系統(tǒng)，并且 內(nèi)存占用量低，推理速度快，比較適合資源受限制的場景 。

▊ 卡爾曼濾波介紹

先來看一個(gè)簡單的問題。考慮一輛車在道路上行駛，他的狀態(tài)和運(yùn)行速度，位置信息相關(guān)，用向量表示為：

其中 p 變量對應(yīng)位置 v 變量對應(yīng)速度，都為隨機(jī)變量且符合正態(tài)分布，每個(gè)分布都有一個(gè)均值（正太分布的中心）和對應(yīng)的方差 .（衡量變量的不確定性）。

那么卡爾曼濾波是怎么工作的呢？我們還是回到剛才的例子。上面例子中的這兩個(gè)變量看起來沒有明顯的對應(yīng)關(guān)系，也就是說，給定速度，沒法準(zhǔn)確定出位置。

但是如果我們知道位置和速度的初始信息，其實(shí)是有辦法將兩個(gè)變量聯(lián)系起來的。假設(shè)車輛一直保持勻速行駛，通過下面的公式其實(shí)就可以（要注意的是，換成其他問題，我們不一定能意識到這個(gè)公式的存在）

基于上面的公式，我們可以直觀的定義當(dāng)前時(shí)刻和上一時(shí)刻之間的對應(yīng)關(guān)系；假定系統(tǒng)中所有狀態(tài)量為

,對應(yīng)的有

這里我們稱

為預(yù)測矩陣，也會(huì)用

表示，預(yù)測矩陣的目的是刻畫從上一狀態(tài)到當(dāng)前狀態(tài)對應(yīng)的關(guān)系。

這也就是卡爾曼濾波的目標(biāo)。我們希望從不確定的系統(tǒng)中，盡可能的發(fā)掘出確定的信息。那么另一個(gè)問題是，如何知道有多少確定（不確定）的信息可以發(fā)掘呢？我們需要一個(gè)指標(biāo)來衡量系統(tǒng)的不確定性。

回到上個(gè)例子中，位置和速度的關(guān)系其實(shí)是不確定的，但是卻是有相關(guān)性的，只是不能直觀的看出來。那么如何衡量這種相關(guān)性呢？其中一個(gè)辦法就是使用協(xié)方差矩陣，來衡量兩個(gè)變量的相關(guān)程度。給定

,對應(yīng)的協(xié)方差矩陣p為

考慮到預(yù)測矩陣的影響，給定上一時(shí)刻的

,對應(yīng)可以更新為

由此便解決了理想情況下卡爾曼濾波如何做預(yù)測的問題。但是實(shí)際情況下，系統(tǒng)中可能還是存在干擾或者噪聲因素。比如車輛行駛過程中，遇到雨天路滑則需要減速慢行，這種情況下道路狀況會(huì)作為外部因素，干擾系統(tǒng)行為和決策。因?yàn)獒槍ψ兓?/p>

,會(huì)額外追加一個(gè)控制變量來建模外部因素。

假設(shè)行車遇到雨天需要減速，我們考慮了運(yùn)動(dòng)時(shí)刻的車輛加速度a為外部影響,那么有，

對比之前計(jì)算狀態(tài)的關(guān)系式，可以發(fā)現(xiàn)新引入的狀態(tài)對系統(tǒng)產(chǎn)生了影響，因而記為

同時(shí)我們使用

表示，引入的外部因素對于協(xié)方差的影響，因而有

這里總結(jié)一下，新引入的外部變量使得

1)當(dāng)前的最佳估計(jì)不再為系統(tǒng)自身的最佳估計(jì)，還考慮了外部因素影響

2)當(dāng)前的不確定性不再為系統(tǒng)自身的不確定性，引入了外部的不確定性做參考