卡爾曼濾波器是一種最優線性狀態估計方法（等價於「在最小均方誤差準則下的最佳線性濾波器」），所謂狀態估計就是通過數學方法尋求與觀測資料最佳擬合的狀態向量。
在移動機械人導航方面，卡爾曼濾波是最常用的狀態估計方法。直觀上來講，卡爾曼濾波器在這裡起了資料融合的作用，只需要輸入當前的測量值（多個感測器資料）和上乙個週期的估計值就能估計當前的狀態，這個估計出來的當前狀態綜合考量了感測器資料（即所謂的觀察值、測量值）和上一狀態的資料，為當前最優估計，可以認為這個估計出來的值是最可靠的值。由於我們在slam中主要用它做位置估計，所以前面所謂的估計值就是估計位置座標了，而輸入的感測器資料報括碼盤推算的位置、陀螺儀的角速度等（當然可以有多個陀螺儀和碼盤），最後輸出的最優估計用來作為機械人的當前位置被導航演算法以外的其他程式所呼叫。
列舉一下卡爾曼濾波的優點：採用遞迴方法解決線性濾波問題，只需要當前的測量值和前乙個取樣週期的估計值就能夠進行狀態估計，不需要大量的儲存空間，每一步的計算量小，計算步驟清晰，非常適合計算機處理。
首先明確卡爾曼濾波器的前提假設：
滿足上述條件就可以使用卡爾曼濾波器。
定義乙個隨機離散時間過程的狀態向量 ，該過程用乙個離散隨機差分方程描述：
其中n維向量
xk為k時刻的系統狀態變數，n維向量
xk−1
是k-1時刻的系統狀態變數。a是狀態轉移矩陣或者過程增益矩陣，是
n×n階方陣，它將k-1時刻狀態和當前的k時刻狀態聯絡起來。b是可選的控制輸入
u∈rl
的增益，在大多數實際情況下並沒有控制增益，所以
buk−1
這一項很愉快的變成零了。
wk−1
是n維向量，代表過程激勵雜訊，它對應了
xk中每個分量的雜訊，是期望為0，協方差為q的高斯白雜訊，wk~
n(0,q)
。 再定義乙個觀測變數 ，得到觀測方程：
其中觀測值
zk是m階向量，狀態變數
xk是n階向量。h是
m×n階矩陣，代表狀態變數
xk對測量變數
zk的增益。觀測雜訊
vk是期望為0，協方差為r的高斯白雜訊，vk~
n(0,r)
。如果對上面兩個公式不太明白，我舉乙個例子說明：
在目標跟蹤的應用中，假設質點座標為 (x,y)是直接觀測得到的，質點在x、y軸方向速度分別為vx 、vy，那麼系統狀態變數 xk=(x,y,vx,vy)t，系統觀測變數 zk=(x¯,y¯)t ，系統沒有控制輸入，所以狀態方程就成了：
它的狀態轉移矩陣a根據運動學公式確定：
其實用矩陣的形式寫開上面的方程就很好理解了：
該過程的觀測方程為
它的觀測矩陣h也是要指定的，它的目的是將m維的測量值轉換到n維與狀態變數相對應，由於直接觀測的量是位置
zk=(x¯,y¯)t
，我們只需要取狀態變數的前兩個元素就夠了，所以h設計成如下：
用矩陣形式寫開就是
接下來就開始介紹卡爾曼濾波最核心的五個更新方程了
x^k、x^k−1：分別表示k時刻、k-1時刻後驗狀態估計值，也就是要輸出的該時刻最優估計值，這個值是卡爾曼濾波的結果。
a：表示狀態轉移矩陣，是n×n 階方陣，它是演算法對狀態變數進行**的依據，狀態轉移矩陣如果不符合目標模型有可能導致濾波發散，它的確定請參看第二節中的舉例。
b：表示可選的控制輸入 u∈rl的增益，在大多數實際情況下並沒有控制增益。
uk−1：表示k-1時刻的控制增益，一般沒有這個變數，可以設為0。
p^k¯：表示k時刻的先驗估計協方差，這個協方差矩陣只要確定了一開始的p^0 ，後面都可以遞推出來，而且初始協方差矩陣p^0 只要不是為0，它的取值對濾波效果影響很小，都能很快收斂。
p^k、p^k−1：分別表示k時刻、k-1時刻的後驗估計協方差，是濾波結果之一。
q：表示過程激勵雜訊的協方差，它是狀態轉移矩陣與實際過程之間的誤差。這個矩陣是卡爾曼濾波中比較難確定的乙個量，一般有兩種思路：一是在某些穩定的過程可以假定它是固定的矩陣，通過尋找最優的q值使濾波器獲得更好的效能，這是調整濾波器引數的主要手段，q一般是對角陣，且對角線上的值很小，便於快速收斂；二是在自適應卡爾曼濾波（akf）中q矩陣是隨時間變化的。
kk：表示卡爾曼增益，是濾波的中間結果。
zk：表示測量值，是m階向量。
h：表示量測矩陣，是m×n 階矩陣，它把m維測量值轉換到n維與狀態變數相對應。
r：表示測量雜訊協方差，它是乙個數值，這是和儀器相關的乙個特性，作為已知條件輸入濾波器。需要注意的是這個值過大過小都會使濾波效果變差，且r取值越小收斂越快，所以可以通過實驗手段尋找合適的r值再利用它進行真實的濾波。
以下是一段卡爾曼濾波與平滑濾波的對比
clear
clc;
n=300;
con = 25;%房間溫度，假定溫度是恆定的
%%%%%%%%%%%%%%%卡爾曼濾波%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,n);
y = 2^0.5 * randn(1,n) + con;%加過程雜訊的測量值
x(1) = 1;
p = 10;
q = cov(randn(1,n));%過程雜訊協方差
r = cov(randn(1,n));%觀測雜訊協方差
for k = 2 : n
x(k) = x(k - 1);%預估計k時刻狀態變數的值
p = p + q;%對應於預估值的協方差，由於狀態變數是標量，狀態轉移矩陣a為數值1
kg = p / (p + r);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
end%%%%%%%%%%%平滑濾波%%%%%%%%%%%%%%%%%%%%%%%%
[1] 王學斌, 徐建巨集, 張章. 卡爾曼濾波器引數分析與應用方法研究[j]. 計算機應用與軟體, 2012, 29(6):212-215.
卡爾曼濾波的理解以及引數調整
一 前言 卡爾曼濾波器是一種最優線性狀態估計方法 等價於 在最小均方誤差準則下的最佳線性濾波器 所謂狀態估計就是通過數學方法尋求與觀測資料最佳擬合的狀態向量。在移動機械人導航方面，卡爾曼濾波是最常用的狀態估計方法。直觀上來講，卡爾曼濾波器在這裡起了資料融合的作用，只需要輸入當前的測量值 多個感測器資...
卡爾曼濾波的理解
首先了解一下濾波 濾波 filtering is weighting 濾波即加權 濾波的作用就是給不同的訊號分量不同的權重。最簡單的loss pass filter，就是直接把低頻的訊號給0權重，而給高頻部分1權重。常見的貝葉斯濾波和卡爾曼濾波 貝葉斯濾波我覺得關鍵在於對先驗概率和後驗概率的理解上，...
卡爾曼濾波學習理解
首先，我們引入乙個離散控制過程的系統。該系統可用乙個線性隨機微分方程來描述 x k ax k 1 bu k w k z k hx k v k x k 是k時刻的系統狀態，u k 是k時刻對系統的控制量。a和b是系統引數，對於多模型系統，他們為矩陣。z k 是k時刻的測量值，h是測量系統的引數，對於多...