卡爾曼濾波在感測器資料融合 ，狀態**裡佔據了很重要的地位，今天有空稍微整理一下，整理自己的思路，加深理解記憶。要逐漸的把kf，ekf，ｐｆ，ｕｋｆ都理解清楚，熟悉他們的優劣勢，明確應用場合，熟能生巧，善莫大焉。
最佳線性濾波理論起源於40年代美國科學家wiener和前蘇聯科學家ｋолмогоров等人的研究工作，後人統稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的資料，不適用於實時處理。為了克服這一缺點，60年代kalman把狀態空間模型引入濾波理論，並匯出了一套遞推估計演算法，後人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計的最佳準則，來尋求一套遞推估計的演算法，其基本思想是：採用訊號與雜訊的狀態空間模型，利用前一時刻地估計值和現時刻的觀測值來更新對狀態變數的估計，求出現時刻的估計值。它適合於實時處理和計算機運算。
線性隨機微分方程為：
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是測量系統的引數，對於多測量系統，h為矩陣。w(k)和v(k)分別表示過程和測量的雜訊。他們被假設成高斯白雜訊(white gaussian noise)，他們的covariance 分別是q，r（這裡我們假設他們不隨系統狀態變化而變化）。對於滿足(線性隨機微分系統，過程和測量都是高斯白雜訊)的條件，卡爾曼濾波器是最優的資訊處理器。
（一）**過程
根據系統的模型，可以基於系統的上一狀態而**出現在狀態：
x(k|k-1)=a x(k-1|k-1)+b u(k) ……….. (1)
式(1)中，x(k|k-1)是利用上一狀態**的結果，x(k-1|k-1)是上一狀態最優的結果，u(k)為現在狀態的控制量，如果沒有控制量，它可以為0。
對應於x(k|k-1)的covariance，我們用p表示covariance，更新公式如下：
p(k|k-1)=a p(k-1|k-1) a』+q ……… (2)
式(2)中，p(k|k-1)是x(k|k-1)對應的covariance，p(k-1|k-1)是x(k-1|k-1)對應的covariance，a』表示a的轉置矩陣，q是系統過程的covariance。式子1，2就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的**。
（二）公升級過程
我們有了現在狀態的**結果，然後我們再收集現在狀態的測量值。結合**值和測量值，我們可以更新得到現在狀態(k)的最優化估算值x(k|k)：
x(k|k)= x(k|k-1)+kg(k) (z(k)-h x(k|k-1)) ……… (3)
其中kg為卡爾曼增益(kalman gain)：
kg(k)= p(k|k-1) h』 / (h p(k|k-1) h』 + r) ……… (4)
我們已經得到了k狀態下最優的估算值x(k|k)。為了要另卡爾曼濾波器不斷的執行下去直到系統過程結束，我們還要更新k狀態下x(k|k)的covariance：
p(k|k)=（i-kg(k) h）p(k|k-1) ……… (5)
其中i 為1的矩陣，對於單模型單測量，i=1。當系統進入k+1狀態時，p(k|k)就是式子(2)的p(k-1|k-1)。這樣，演算法就可以自回歸的運算下去。
（三）設線性時變系統的離散狀態防城和觀測方程為：
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)~
重複以上步驟
卡爾曼 卡爾曼濾波 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...