卡爾曼濾波完整推導
卡爾曼濾波部分我打算分三節（三次部落格的內容）：
卡爾曼濾波以及其擴充套件演算法能夠應用於目標狀態估計，如果這個目標是行人，那麼就是行人狀態估計（或者說行人追蹤），如果這個目標是自身，那麼就是車輛自身的追蹤（結合一些地圖的先驗，gps等資料的話就是自身的定位）。在很多的無人駕駛汽車專案中，都能找到卡爾曼濾波的擴充套件演算法的身影（比如說ekf，ukf等等）。本節我們從最簡單的卡爾曼濾波出發，完整的理解一遍卡爾曼濾波的推導過程，並實現乙個簡單的狀態估計python程式。
我們通常要對一些事物的狀態去做估計，為什麼要做估計呢？因為我們通常無法精確的知道物體當前的狀態。為了估計乙個事物的狀態，我們往往會去測量它，但是我們不能完全相信我們的測量，因為我們的測量是不精準的，它往往會存在一定的雜訊，這個時候我們就要去估計我們的狀態。卡爾曼濾波就是一種結合**（先驗分布）和測量更新（似然）的狀態估計演算法。
先驗概率 p ( x )：僅僅依賴主觀上的經驗，事先根據已有的只是的推斷
後驗概率 p ( x ∣ z )：是在相關證據或者背景給定並納入考慮以後的條件概率
似然 p ( z ∣ x )：已知結果區推測固有性質的可能性
貝葉斯公式：
p(a∣b)= p(a)×p(b∣a) / p(b)
後驗分布正比於先驗分布乘以似然。
乙個簡單的例子
若干年後，我們的可**火箭要降落到地球，我們比較關心的狀態就是我們的飛行器的高度了，飛行器的高度就是我們想要估計的狀態，我們會通過一些感測器去測量我們當前的高度資訊，比如說使用氣壓計。假如我們每一次測量，當前高度都變成上一次測量的 95 %，那麼我們就可以得到如下關係：
height(t)=0.95×height (t−1)
我們可以使用遞迴來表示這樣的公式——為了計算我們當前的高度，我們必須知道我們上乙個測量的高度，以此遞推，我們就會推到我們的飛行器的初始高度。
但是，由於我們的測量往往來自於一些感測器（比如說gps，氣壓計），所以測量的結果總是帶有雜訊的，這個雜訊是有感測器本身引起的，那麼我們的表示式就變成了：
measurement (t) =height(t) + noise (t)
這個雜訊我們稱之為測量雜訊 (measuremen noise)。通常來說，這種雜訊都滿足高斯分布。我們用符號描述以上兩部分：
第乙個式子是我們的 過程模型，是我們的經驗（比如說一些運動模型，牛頓力學等等），我們用這種過程模型去 ** 我們考察的事物的狀態（在沒有任何資訊的情況下，或者說在沒有任何測量資料的情況下）；第二個式子是測量的表示式，它大致描述了我們測量的組成成分，我們用這個測量去 更新 我們對狀態的估計。 其中 xk是我們的飛行器當前的狀態， xk-1 是我們上乙個狀態（注意狀態和測量的區別），zk是我們當前對飛行器的測量，vk使我們當前的測量雜訊， a 是乙個常數，在我們這個例子裡面就是 0.95。
很顯然，現實中的運動不會像我們這個簡單的過程模型一樣高度按比例縮小。在這裡為了簡化，我們先假設我們的模型挺好的，能夠大致描述出這顆「神奇火箭」的運動規律，只是偶爾會存在一定的偏差（比如所空氣湍流的影響），那麼我們在計 xk時在加乙個雜訊來描述我們的過程模型與實際運動的差異，這個雜訊我們稱之為過程雜訊 (process noise)，我們用 wk 來表示這種雜訊，那麼xk 的計算公式就變成：
xk =axk−1+wk
為了簡化，後面的分析我們會先忽略過程雜訊，但是我們在感測器融合的部分會將過程雜訊重新考慮進來。
因為我們是要估計飛行器的狀態（高度），所以我們把測量的公式變換成：
xk =zk-vk
顯然，在這裡 vk 是沒辦法知道的，卡爾曼濾波通過同時考慮上一狀態值和當前的測量值來獲得對當前狀態值的估計。一般我們用 x
^\hat
x^來表示對狀態 x 的估計。那麼x
^\hat
x^k就表示當前狀態的估計。下面我們可以用如下公式來描述卡爾曼濾波如何結合上乙個估計和和現在的測量來產生對當前的估計：
這裡的 gk 叫做 卡爾曼增益（kalman gain），它描述的是之前的估計和當前的測量對當前的估計的影響的分配權重。為了理解，我們考慮極端的例子，如果 gk = 0 ,也就是說增益為 0，那麼：
x
^\hat
x^k = zk
即我認為當前的測量非常可信，我徹底接受它作為我當前狀態的估計。那麼當 gk 介於 ( 0 , 1 )，則表示對兩者的權重分配。
那麼如何計算卡爾曼增益呢？我們使用一種間接的方法，我們雖然不知道測量雜訊 vk 的值，但是我們知道它的均值，前面我們提到，測量雜訊來自感測器本身，並且符合高斯分布，所以我們能夠從感測器廠商那裡獲得測量雜訊的均值 r ，那麼 gk可以表示為：
gk=pk-1 /(pk-1 + r)
其中 pk 叫**誤差，表示式為：
pk = (1 - gk ) pk-1
那麼怎麼理解 pk和 gk呢？
那麼假設前一次的**誤差 pk-1=0， 那麼根據公式，當前的增益gk=0，一維著捨棄掉當前的測量而完全採用上乙個時刻的估計，如果pk-1=1, 那麼增益變成 1 / (1 + r)通常 r 是個很小的數值，所以增益為1，所以完全接受這一次的測量作為我們的估計（因為上一次的的**誤差太大了，為1，所以一旦拿到了新的測量，如獲至寶，就乾脆把不準確的上次的估計捨棄掉了）.
對於下面的公式的分析是一樣的，我們考慮極端的例子，當增益為 0 ，pk = pk-1，因為我們徹底捨棄掉了本次的測量，所以本次的**誤差只能接受上一次的。當增益為 1 ，pk= 0 。
那麼現在我們有關於當前的火箭高度狀態的兩個公式了，它們分別是：
xk = a xk-1
和那麼我們到底要用哪乙個呢？答案是我們都用，第乙個公式我們稱之為**，是基於一些先驗的知識（比如說運動模型，牛頓力學等等）覺得我們的狀態應該是這樣的，而第二個公式呢，就是我們基於我們「不完美的」的感測器的測量資料來更新我們對狀態的估計。另為，**，理論上只考慮了乙個固定的過程模型和過程雜訊，但是由於我們現在是對機械的狀態進行估計，在**過程中需要對機械本身的控制建模， 我們在**部分再新增乙個控制訊號，我們用 b uk 表示。實際的感測器測量除了會有測量雜訊vk 以外，還會存在一定的關於真實狀態的縮放，因此我們使用 xk 表示測量時通常還會在其前面加乙個縮放係數 c。 結合這寫我們就可以得到卡爾曼濾波**和更新過程了：
卡爾曼濾波更新的過程為：
使用線性代數的方法來表示**和更新
卡爾曼濾波更新的過程為：
至此，卡爾曼濾波的完整推導就結束了。下面，我們來看看卡爾曼濾波在無人汽車的感知模組的應用。
卡爾曼濾波演算法為什麼會叫濾波演算法？
參考：
卡爾曼 卡爾曼濾波 1
今天主要介紹一下卡爾曼濾波器，所謂卡爾曼濾波器其實是一種最優化遞迴數字處理演算法 optimal recursive data processing algorithm 卡爾曼濾波器應用 既然我們有了測量儀器，這些測量儀器可以目標給出準確測量值。還需要卡爾曼濾波器進行估計嗎?下面解釋一下為什麼需要卡...
卡爾曼 基礎卡爾曼濾波
卡爾曼濾波器是一種基礎 定位演算法。原理非常簡單易懂。核心過程可以用乙個圖說明 本質上就是這兩個狀態過程的迭代，來逐步的準確定位。更新 更具感測器獲取到比較準確的位置資訊後來更新當前的 問位置，也就是糾正 的錯誤。你可能要問為什麼有感測器的資料了還要進行更新？因為在現實世界中感測器是存在很多雜訊干擾...
卡爾曼濾波
卡爾曼濾波演算法 首先引入乙個離散控制過程的系統，用乙個線性隨機微分方程來描述 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...