整篇轉自：https://blog.csdn.net/zkl99999/article/details/46619771/
轉自http://blog.csdn.net/karen99/article/details/7771743
卡爾曼濾波本來是控制系統課上學的，當時就沒學明白，也矇混過關了，以爲以後也不用再見到它了，可惜沒這麼容易，後來學計算機視覺和圖像處理，發現用它的地方更多了，沒辦法的時候只好耐心學習和理解了。一直很想把學習的過程記錄一下，讓大家少走彎路，可惜總也沒時間和機會，直到今天。。。
我一直有一個願望，就是把抽象的理論具體化，用最直白的方式告訴大家--不提一個生澀的詞，不寫一個數學公式，像講故事一樣先把道理說明白，需要知道細節的同學可以自己去查所有需要知道的一切。因爲學習的過程告訴我，最難的其實是最初和這個理論和應用背景親和的過程--這些理論它究竟是做什麼的，又是怎麼做到的。可惜我們能看到的關於這些理論的資料大多數都是公式的堆砌並且假定我們明白許多“基本的道理”，其實這些“基本的道理”往往是我們最難想象和超越的。以卡爾曼濾波爲例，讓我們嘗試一種不同的學習方法。
相信所有學習卡爾曼濾波的同學首先接觸的都是狀態方程和觀測方程，學過控制系統的同學可能不陌生，否則，先被那兩個看起來好深奧的公式給嚇跑了，關鍵是還不知道他們究竟是幹什麼的，什麼是狀態，什麼是觀測。。。。。。如果再看到後面的一大串遞歸推導增益，實在很暈很暈，更糟糕的是還沒整明白的時候就已經知道卡爾曼濾波其實已經不夠使了，需要extended kalmanfilter 和particle filter了。。。
其實我們完全不用理會這些公式。先來看看究竟卡爾曼濾波是做什麼的，理解了卡爾曼濾波，下面的就順其自然了。
用一句最簡單的話來說，卡爾曼濾波是來幫助我們做測量的，大家一定不明白測量幹嘛搞那麼複雜？測量長度拿個尺子比一下，測量溫度拿溫度表測一下不就完了嘛。的確如此，如果你要測量的東西很容易測準確，沒有什麼隨機干擾，那真的不需要勞駕卡爾曼先生。但在有的時候，我們的測量因爲隨機干擾，無法準確得到，卡爾曼先生就給我們想了個辦法，讓我們在干擾爲高斯分佈的情況下，得到的測量均方誤差最小，也就是測量值擾動最小，看起來最平滑。
還是舉例子最容易明白。我最近養了只小兔子，忍不住拿小兔子做個例子嘻嘻。
每天給兔子拔草，看她香甜地喫啊喫地，就忍不住關心一下她的體重增長情況。那麼我們就以小兔子的體重作爲研究對象吧。假定我每週做一次觀察，我有兩個辦法可以知道兔子的體重，一個是拿體重計來稱：或許你有辦法一下子就稱準兔子的體重（獸醫通常都有這辦法），但現在爲了體現卡爾曼先生理論的魅力，我們假定你的稱實在很糟糕，誤差很大，或者兔子太調皮，不能老實待著，彈簧秤因爲小兔子的晃動會產生很大誤差。儘管有誤差，那也是一個不可失去的渠道來得到兔子的體重。還有一個途徑是根據書本上的資料，和兔子的年齡，我可以估計一下我的小兔子應該會多重，我們把用稱稱出來的叫觀察量，用資料估計出來的叫估計值，無論是觀察值還是估計值顯然都是有誤差的，假定誤差是高斯分佈。現在問題就來了，按照書本上說我的兔子該3公斤重，稱出來卻只有2.5公斤，我究竟該信哪個呢？如果稱足夠準，兔子足夠乖，卡爾曼先生就沒有用武之地了呵呵，再強調一下是我們的現狀是兔兔不夠乖，稱還很爛呵呵。在這樣惡劣的情景下，卡爾曼先生告訴我們一個辦法，仍然可以估計出八九不離十的兔兔體重，這個辦法其實也很直白，就是加權平均，把稱稱出來的結果也就是觀測值和按照書本經驗估算出來的結果也就是估計值分別加一個權值，再做平均。當然這兩個權值加起來是等於一的。也就是說如果你有0.7分相信稱出來的體重，那麼就只有0.3分相信書上的估計。說到這裏大家一定更着急了，究竟該有幾分相信書上的，有幾分相信我自己稱的呢？都怪我的稱不爭氣，沒法讓我百分一百信賴它，還要根據書上的數據來做調整。好在卡爾曼先生也體會到了我們的苦惱，告訴我們一個辦法來決定這個權值，這個辦法其實也很直白，就是根據以往的表現來做決定，這其實聽起來挺公平的，你以前表現好，我就相信你多一點，權值也就給的高一點，以前表現不好，我就相信你少一點，權值自然給的低一點。那麼什麼是表現好表現不好呢，表現好意思就是測量結果穩定，方差很小，表現不好就是估計值或觀測值不穩定，方差很大。想象你用稱稱你的哦兔子，第一次1公斤第二次10公斤，第三次5公斤，你會相信你的稱嗎，但是如果第一次3公斤第二次3.2公斤，第三次2.8公斤，自然我就相信它多一點，給它一個大的權值了。
有了這個權值，下面的事情就很好辦了。很顯然卡爾曼先生是利用多次觀察和估計來達到目的的，我們也只能一步一步地調整我們的觀察和估計值，來漸漸達到準確的測量，所以整個算法是遞歸的，需要多次重複調整的。調整的過程也很簡單，就是把實測值（稱出來的體重）和估計值（書上得來的體重）比較一下，如果估計值比測量值小，那就把估計值加上他們之間的偏差作爲新的估計值，當然前面要加個係數，就是我們前面說的加權係數，這個地方我要寫個公式，因爲很簡單就能說明白
比如我們的觀查值是Z，估計值是X， 那麼新的估計值就應該是 Xnew = X + K ( Z-X），從這個公式可以看到，如果X估計小了，那麼新的估計值會加上一個量K ( Z-X）, 如果估計值大了,大過Z了，那麼新的估計值就會減去一個量K ( Z-X），這就保證新的估計值一定比現在的準確，一次一次遞歸下去就會越來越準卻了，當然這裏面很有作用的也是這個K，也就是我們前面說的權值，書上都把他叫卡爾曼增益。。。(Xnew = X + K ( Z-X） = X ×（1-K) + KZ ，也就是說估計值X的權值是1-k，而觀察值Z的權值是k，究竟k 取多大，全看估計值和觀察值以前的表現，也就是他們的方差情況了）
發現把一個問題講明白還真不是件容易的事情，誰聽明白了我佩服誰，因爲我已經把自己講糊塗了哈
順便就把extended kalman filter和particle filter提一下，因爲高斯模型有時不適用，於是有了extended kalman filter，而particle filter是用於多個對象的，比如除了兔子我還有隻貓，他們的體重有一個聯合概率模型，每一個對象就是一個particle。無論是卡爾曼濾波還是particle濾波，都是概率分佈傳遞的過程，卡爾曼傳遞的是高斯分佈，particle filter 傳遞的是高斯混合分佈，每一個峯代表一個動物在我們的例子。
轉自：http://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html#undefined
上學的時候每次遇到“粒子濾波”那一堆符號，我就暈菜。今天閒來無事，搜了一些文章看，終於算是理解了。下面用白話記一下我的理解。
問題表述：
某年月，警方（跟蹤程序）要在某個城市的茫茫人海（採樣空間）中跟蹤尋找一個罪犯（目標），警方採用了粒子濾波的方法。
1. 初始化：
警方找來了一批警犬（粒子），並且讓每個警犬預先都聞了罪犯留下來的衣服的味道（爲每個粒子初始化狀態向量S0），然後將警犬均勻佈置到城市的各個區（均勻分佈是初始化粒子的一種方法，另外還有諸如高斯分佈，即：將警犬以罪犯留衣服的那個區爲中心來擴展分佈開來）。
2. 搜索：
每個警犬都聞一聞自己位置的人的味道（粒子狀態向量Si），並且確定這個味道跟預先聞過的味道的相似度（計算特徵向量的相似性），這個相似度的計算最簡單的方法就是計算一個歐式距離（每個粒子i對應一個相似度Di），然後做歸一化（即：保證所有粒子的相似度之和爲1）。
3. 決策：
總部根據警犬們發來的味道相似度確定罪犯出現的位置（概率上最大的目標）：最簡單的決策方法爲哪個味道的相似度最高，那個警犬處的人就是目標。
4. 重採樣：
總部根據上一次的決策結果，重新佈置下一輪警犬分佈（重採樣過程）。最簡單的方法爲：把相似度比較小的地區的警犬抽調到相似度高的地區。
上述，2,3,4過程重複進行，就完成了粒子濾波跟蹤算法的全過程。
一直都覺得粒子濾波是個挺牛的東西，每次試圖看文獻都被複雜的數學符號搞得看不下去。一個偶然的機會發現了Rob Hess(http://web.engr.oregonstate.edu/~hess/)實現的這個粒子濾波。從代碼入手，一下子就明白了粒子濾波的原理。根據維基百科上對粒子濾波的介紹(http://en.wikipedia.org/wiki/Particle_filter)，粒子濾波其實有很多變種，Rob Hess實現的這種應該是最基本的一種，Sampling Importance Resampling (SIR)，根據重要性重採樣。下面是我對粒子濾波實現物體跟蹤的算法原理的粗淺理解：
1）初始化階段-提取跟蹤目標特徵
該階段要人工指定跟蹤目標，程序計算跟蹤目標的特徵，比如可以採用目標的顏色特徵。具體到Rob Hess的代碼，開始時需要人工用鼠標拖動出一個跟蹤區域，然後程序自動計算該區域色調(Hue)空間的直方圖，即爲目標的特徵。直方圖可以用一個向量來表示，所以目標特徵就是一個N*1的向量V。
2）搜索階段-放狗
好，我們已經掌握了目標的特徵，下面放出很多條狗，去搜索目標對象，這裏的狗就是粒子particle。狗有很多種放法。比如，a)均勻的放：即在整個圖像平面均勻的撒粒子(uniform distribution)；b)在上一幀得到的目標附近按照高斯分佈來放，可以理解成，靠近目標的地方多放，遠離目標的地方少放。Rob Hess的代碼用的是後一種方法。狗放出去後，每條狗怎麼搜索目標呢？就是按照初始化階段得到的目標特徵(色調直方圖，向量V)。每條狗計算它所處的位置處圖像的顏色特徵，得到一個色調直方圖，向量Vi，計算該直方圖與目標直方圖的相似性。相似性有多種度量，最簡單的一種是計算sum(abs(Vi-V)).每條狗算出相似度後再做一次歸一化，使得所有的狗得到的相似度加起來等於1.
3）決策階段
我們放出去的一條條聰明的狗向我們發回報告，“一號狗處圖像與目標的相似度是0.3”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”...那麼目標究竟最可能在哪裏呢？我們做次加權平均吧。設N號狗的圖像像素座標是(Xn,Yn),它報告的相似度是Wn,於是目標最可能的像素座標X = sum(Xn*Wn),Y = sum(Yn*Wn).
4）重採樣階段Resampling
既然我們是在做目標跟蹤，一般說來，目標是跑來跑去亂動的。在新的一幀圖像裏，目標可能在哪裏呢？還是讓我們放狗搜索吧。但現在應該怎樣放狗呢？讓我們重溫下狗狗們的報告吧。“一號狗處圖像與目標的相似度是0.3”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”...綜合所有狗的報告，一號狗處的相似度最高，三號狗處的相似度最低，於是我們要重新分佈警力，正所謂好鋼用在刀刃上，我們在相似度最高的狗那裏放更多條狗，在相似度最低的狗那裏少放狗，甚至把原來那條狗也撤回來。這就是Sampling Importance Resampling，根據重要性重採樣(更具重要性重新放狗)。
(2)->(3)->(4)->(2)如是反覆循環，即完成了目標的動態跟蹤。
根據我的粗淺理解，粒子濾波的核心思想是隨機採樣+重要性重採樣。既然我不知道目標在哪裏，那我就隨機的撒粒子吧。撒完粒子後，根據特徵相似度計算每個粒子的重要性，然後在重要的地方多撒粒子，不重要的地方少撒粒子。所以說粒子濾波較之蒙特卡洛濾波，計算量較小。這個思想和RANSAC算法真是不謀而合。RANSAC的思想也是(比如用在最簡單的直線擬合上)，既然我不知道直線方程是什麼，那我就隨機的取兩個點先算個直線出來，然後再看有多少點符合我的這條直線。哪條直線能獲得最多的點的支持，哪條直線就是目標直線。想法非常簡單，但效果很好。
扯遠了，下面還是說說代碼吧。Rob Hess的代碼好像是linux上的，我稍微改了下，讓Windows+VS2008能跑。main函數在track1.c裏，默認是處理附帶的視頻，當然可以方便的改成從攝像頭輸入視頻(cvCaptureFromCAM)。我用的是opencv2.0，沒有測試低於2.0的版本。程序開始後，讀入第一幀圖像後，需要用鼠標拖動出跟蹤目標區域，(然後按回車)。跟蹤區域較小時速度較快，跟蹤區域大時明顯變得很卡。代碼還需要gsl庫的支持，我用的是gsl1.8,下面提供了gsl1.8安裝文件的下載。這裏是全部代碼和相關的VS2008工程文件
轉自：http://skpsun.blog.163.com/blog/static/2760055201010251210841/
這裏主要講計算機視覺範疇下的Kalman Filter（KF）和Particle Filter（PF）。
Kalman Filter建立在高斯模型上的，這是說目標的model，即狀態噪音的概率密度函數（state probability density）和觀察模型（observation model）是高斯分佈。Kalman的數學基礎是指，將兩個高斯模型結合起來時，將得到一個新的高斯模型，並且新高斯模型的參數（均值和方差）可完全由原來兩個高斯模型的參數計算得到。應用在時間序列中，就是說，知道了t-1時刻和t時刻state噪音的分佈（符合高斯模型），就可算出t+1時刻state的分佈。從而實現迭代運算。
同時，Kalman Filter又是線性的，t時刻的state可以由t-1時刻的state經過一個矩陣相乘得到。
前面提到，噪音是高斯白噪音，白噪音是指其分佈和時間不相關。噪音分爲兩個部分，狀態量中的噪音和觀測量中的噪音。即分別爲下述式（1）的W(t)和和式（2）中V(k)。
所以在高斯和線性兩個假設之下，我們既可以得到state的取值（由線性假設得到），又可以得到state取值的概率密度（由高斯假設得到），這時就可以通過迭代實現對系統每一時刻狀態的預測。再結合（assimilate）上觀測值，對預測結果糾正，從而實現一個完整的跟隨預測系統。也就是說，卡爾曼濾波器就是一個信息融合器，把我們不準確的預測結果和不準確的測量結果融合在一起，得到一個估計結果。
以上過程可由一個線性隨機微分方程（Linear Stochastic Difference equation）來描述：
X(t)=F X(t-1)+B U(t)+W(t) -------------------------式（1）
再加上系統的測量值：
Z(t)=H X(t)+V(t) ------------------------------------式（2）
上 兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。F（dynamic model或transition model）和B（control model）是系統參數，對於state vector維度高於一的系統，他們爲矩陣。Z(k)是k時刻的測量值，H 是測量系統的參數，對於多測量系統，H爲矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R。這裏要注意區分狀態量（state）X(t)和觀測量（measurement）Z(t)的區別。比如若state是追蹤目標的圖像位置(x,y)以及速度v，即state vector是一個三維向量，而觀測時可能只能對目標的位置進行直接觀測，即觀測量是(x,y)這樣一個二維向量。這兩者的不同，體現在矩陣F和H上。
Kalman Filter中的高斯和線性假設有時並不能滿足實際情況，因爲高斯模型是單峯的（uni-modal）。比如當一個運動目標被遮擋後，我們不知道它將在遮擋物的後面做什麼運動，是繼續前進，或是停下，還是轉向或後退。這種情況下，Kalman Filter只能給出一個位置的預測，至多加大這個預測的不確定性（即增大協方差矩陣，或，若state是單維度時的方差）。這種情況下，預測位置的不確定噪音事實上已不是高斯模型，它將具有多個峯值（multi-modal）。而且，這種噪音常常無法解析表達。這就引入了Particle Filter。
Particle Filter是基於蒙特卡洛方法（Monte Carlo Method），簡言之就是用點來表達概率密度函數。點密集的地方，概率密度就大。在時間序列中，就是時間序列蒙特卡洛（Sequential Monte Carlo）。所以Particle Filter在機器視覺中的應用，稱爲CONDENSATION（Conditional Density Propagation），它和Bootstrap Filter一樣，同屬於Sequential Monte Carlo範疇。
具體實施上，PF對state的更新不再採用KF中的高斯模型更新，而是採用factored sampling方法。簡單說就是對t-1時刻的所有Particle，根據每個Particle的概率，重新對他們採樣。高概率的Particle將得到高的採樣機率，而低概率的Particle對應低的採樣機率。這樣，高概率Particle將可能被多次採樣，而低概率Particle可能就被放棄。這樣得到t時刻的Particle。然後將t時刻每一個Particle所對應的測量值結合起來，爲t時刻的Particle重新賦以新的概率，以用於t+1時刻新Particle的生成。
所以可以總結， KF和PF相同的是，都分爲三個步驟：Prediction，Measurement和Assimilation（或稱爲correction）。只是每步的實現上不同。
在Prediction階段，KF利用高斯模型進行預測，而PF採用factored sampling對原Particle重採樣。
在Measurement階段，KF將得到唯一的measurement，而PF將爲每一個Particle得到一份measurement。
在Assimilation階段，KF將由state計算得到的觀測值和實際觀測值比較結合，得到更新後的系統參數。而PF通過比較 每個Particle所對應的觀測值和模型預測值之間的差別，更新每個Particle的概率。
所以，KF和PF最大的不同就在於對狀態概率密度函數的表達方式上。KF採用高斯模型，而PF利用用無參數的點來近似。
參考文獻：
1、卡爾曼濾波器：http://www.china-vision.net/blog/user1/6/2006111115624.html
2、Learning OpenCV --- Computer Vision with the OpenCV Library
3、Michael Isard and Andrew Blake，CONDENSATION—Conditional Density Propagation for Visual Tracking，IJCV 1998
4、Michael Isard and Andrew Blake ，Contour Tracking by Stochastic Propagation of Conditional Density，ECCV，1996
5、Aly Merchant, Keir Mierle ，A Simple Implementation of the Condensasion Algorithm，2005
6、Arnaud Doucet , Simon Godsill , Christophe Andrieu ，On Sequential Monte Carlo sampling methods for Bayesian filtering，Statistics and Computing，2000