相信所有學習卡爾曼濾波的同學首先接觸的都是狀態方程和觀測方程，學過控制系統的同學可能不陌生，否則，先被那兩個看起來好深奧的公式給嚇跑了，關鍵是還不知道他們究竟是幹什麼的，什麼是狀態，什麼是觀測。。。。。。如果再看到後面的一大串遞迴推導增益，實在很暈很暈，更糟糕的是還沒整明白的時候就已經知道卡爾曼濾波其實已經不夠使了，需要extended kalmanfilter 和particle filter了。。。
其實我們完全不用理會這些公式。先來看看究竟卡爾曼濾波是做什麼的，理解了卡爾曼濾波，下面的就順其自然了。
用一句最簡單的話來說，卡爾曼濾波是來幫助我們做測量的，大家一定不明白測量幹嘛搞那麼複雜？測量長度拿個尺子比一下，測量溫度拿溫度表測一下不就完了嘛。的確如此，如果你要測量的東西很容易測準確，沒有什麼隨機干擾，那真的不需要勞駕卡爾曼先生。但在有的時候，我們的測量因為隨機干擾，無法準確得到，卡爾曼先生就給我們想了個辦法，讓我們在干擾為高斯分布的情況下，得到的測量均方誤差最小，也就是測量值擾動最小，看起來最平滑。
還是舉例子最容易明白。我最近養了只小兔子，忍不住拿小兔子做個例子嘻嘻。
有了這個權值，下面的事情就很好辦了。很顯然卡爾曼先生是利用多次觀察和估計來達到目的的，我們也只能一步一步地調整我們的觀察和估計值，來漸漸達到準確的測量，所以整個演算法是遞迴的，需要多次重複調整的。調整的過程也很簡單，就是把實測值（稱出來的體重）和估計值（書上得來的體重）比較一下，如果估計值比測量值小，那就把估計值加上他們之間的偏差作為新的估計值，當然前面要加個係數，就是我們前面說的加權係數，這個地方我要寫個公式，因為很簡單就能說明白
比如我們的觀查值是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 傳遞的是高斯混合分布，每乙個峰代表乙個動物在我們的例子。
假設你有兩個感測器，測的是同乙個訊號。可是它們每次的讀數都不太一樣，怎麼辦？
取平均。
再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？
加權平均。
怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，（此處省略若干數學公式），你可以得到乙個「最優」的權重。
接下來，重點來了：假設你只有乙個感測器，但是你還有乙個數學模型。模型可以幫你算出乙個值，但也不是那麼準。怎麼辦？
把模型算出來的值，和感測器測出的值，（就像兩個感測器那樣），取加權平均。
ok，最後一點說明：你的模型其實只是乙個步長的，也就是說，知道x(k)，我可以求x(k+1)。問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權平均之後的那個、對x在k時刻的最佳估計值。
於是迭代也有了。
這就是卡爾曼濾波。
最佳線性濾波理論起源於40年代美國科學家wiener和前蘇聯科學家ｋолмогоров等人的研究工作，後人統稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的資料，不適用於實時處理。為了克服這一缺點，60年代kalman把狀態空間模型引入濾波理論，並匯出了一套遞推估計演算法，後人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計的最佳準則，來尋求一套遞推估計的演算法，其基本思想是：採用訊號與雜訊的狀態空間模型，利用前一時刻地估計值和現時刻的觀測值來更新對狀態變數的估計，求出現時刻的估計值。它適合於實時處理和計算機運算。
現設線性時變系統的離散狀態防城和觀測方程為：
x(k) = f(k,k-1)·x(k-1)+t(k,k-1)·u(k-1)
y(k) = h(k)·x(k)+n(k) 其中
x(k)和y(k)分別是k時刻的狀態向量和觀測向量
f(k,k-1)為狀態轉移矩陣
u(k)為k時刻動態雜訊
t(k,k-1)為系統控制矩陣
h(k)為k時刻觀測矩陣
n(k)為k時刻觀測雜訊
則卡爾曼濾波的演算法流程為：
預估計x(k)^= f(k,k-1)·x(k-1)
計算預估計協方差矩陣
c(k)^=f(k,k-1)×c(k)×f(k,k-1)'+t(k,k-1)×q(k)×t(k,k-1)'
q(k) = u(k)×u(k)'
計算卡爾曼增益矩陣
k(k) = c(k)^×h(k)'×[h(k)×c(k)^×h(k)'+r(k)]^(-1)
r(k) = n(k)×n(k)'
更新估計
x(k)~=x(k)^+k(k)×[y(k)-h(k)×x(k)^]
計算更新後估計協防差矩陣
c(k)~ = [i-k(k)×h(k)]×c(k)^×[i-k(k)×h(k)]'+k(k)×r(k)×k(k)'
x(k+1) = x(k)~
c(k+1) = c(k)~
重複以上步驟
卡爾曼濾波和粒子濾波最直白的解釋
卡爾曼濾波本來是控制系統課上學的，當時就沒學明白，也蒙混過關了，以為以後也不用再見到它了，可惜沒這麼容易，後來學計算機視覺和影象處理，發現用它的地方更多了，沒辦法的時候只好耐心學習和理解了。一直很想把學習的過程記錄一下，讓大家少走彎路，可惜總也沒時間和機會，直到今天。我一直有乙個願望，就是把抽象的理...
關於卡爾曼濾波
簡單來說，卡爾曼濾波器是乙個 optimal recursive data processing algorithm 最優化自回歸資料處理演算法 對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機械人導航，控制，感測器資料融合甚至在軍事方面的雷達系統以及飛彈...
卡爾曼濾波
卡爾曼濾波演算法 首先引入乙個離散控制過程的系統，用乙個線性隨機微分方程來描述 x k a x k 1 b u k w k 系統的測量值 z k h x k v k x k 是k時刻的系統狀態，u k 是k時刻對系統的控制量。a和b是系統引數，對於多模型系統，他們為矩陣。z k 是k時刻的測量值，h...