一、前言
卡爾曼濾波器是一種最優線性狀態估計方法（等價於「在最小均方誤差準則下的最佳線性濾波器」），所謂狀態估計就是通過數學方法尋求與觀測資料最佳擬合的狀態向量。
在移動機械人導航方面，卡爾曼濾波是最常用的狀態估計方法。直觀上來講，卡爾曼濾波器在這裡起了資料融合的作用，只需要輸入當前的測量值（多個感測器資料）和上乙個週期的估計值就能估計當前的狀態，這個估計出來的當前狀態綜合考量了感測器資料（即所謂的觀察值、測量值）和上一狀態的資料，為當前最優估計，可以認為這個估計出來的值是最可靠的值。由於我們在slam中主要用它做位置估計，所以前面所謂的估計值就是估計位置座標了，而輸入的感測器資料報括碼盤推算的位置、陀螺儀的角速度等（當然可以有多個陀螺儀和碼盤），最後輸出的最優估計用來作為機械人的當前位置被導航演算法以外的其他程式所呼叫。
列舉一下卡爾曼濾波的優點：採用遞迴方法解決線性濾波問題，只需要當前的測量值和前乙個取樣週期的估計值就能夠進行狀態估計，不需要大量的儲存空間，每一步的計算量小，計算步驟清晰，非常適合計算機處理。
二、問題描述與定義
1、前提
首先明確卡爾曼濾波器的前提假設：
資訊過程的足夠精確的模型，是由白雜訊所激發的線性、離散和有限維動態系統（可以是時變的）；
每次測量訊號都包含著附加的白雜訊分量。
滿足上述條件就可以使用卡爾曼濾波器。
2、問題描述
定義乙個隨機離散時間過程的狀態向量 ，該過程用乙個離散隨機差分方程描述：
xk=axk−1+buk−1+wk−1
其中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=hxk+vk
其中觀測值zk是m階向量，狀態變數xk 是n階向量。h是m×n 階矩陣，代表狀態變數xk 對測量變數 zk的增益。觀測雜訊vk 是期望為0，協方差為r的高斯白雜訊，vk~n(0,r) 。
3、舉例
如果對上面兩個公式不太明白，我舉乙個例子說明：
在目標跟蹤的應用中，假設質點座標為 (x,y)
是直接觀測得到的，質點在x、y軸方向速度分別為vx 、vy ，那麼系統狀態變數 xk=(x,y,vx,vy)t，系統觀測變數 zk=(x⎯⎯⎯,y⎯⎯⎯)t
，系統沒有控制輸入，所以狀態方程就成了：
xk=axk−1+wk−1
它的狀態轉移矩陣a根據運動學公式確定：
a=⎡⎣⎢⎢⎢⎢10000100δt0100δt01⎤⎦⎥⎥⎥⎥
其實用矩陣的形式寫開上面的方程就很好理解了：
xk=axk−1+wk−1→⎡⎣⎢⎢⎢⎢xyvxvy⎤⎦⎥⎥⎥⎥k=⎡⎣⎢⎢⎢⎢10000100δt0100δt01⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢xyvxvy⎤⎦⎥⎥⎥⎥k−1+wk−1=⎡⎣⎢⎢⎢⎢x+δt⋅vxy+δt⋅vyvxvy⎤⎦⎥⎥⎥⎥k−1+wk−1
這裡簡要說一下狀態轉移矩陣a的確定：
（1） 在標量卡爾曼濾波中，比如測量值是溫度、濕度，一般認為下乙個時刻該溫度或者濕度維持不變，這種情況下狀態轉移矩陣通常就是標量1。
（2） 在導航和目標跟蹤中卡爾曼濾波常被用來做位置估計，比如勻速直線運動（cv）和勻加速直線運動（ca）：
acv=⎡⎣⎢⎢100t10001⎤⎦⎥⎥,aca=⎡⎣⎢⎢⎢100t10t22t1⎤⎦⎥⎥⎥
在前面的舉例中x,y方向上的運動就被微分近似為勻速直線運動。
（3） 若被估計的過程或觀測變數與過程的關係是非線性的，此時不能夠直接應用卡爾曼濾波（因為不滿足上一節提到的卡爾曼濾波適用的前提必須是線性系統），這個時候擴充套件卡爾曼濾波（ekf）應運而生，它用雅克比矩陣將期望和方差線性化，從而將卡爾曼濾波擴充套件到非線性系統，但是ekf由於考慮了泰勒級數的展開，運算量大大增加。
該過程的觀測方程為
zk=hxk+vk
它的觀測矩陣h也是要指定的，它的目的是將m維的測量值轉換到n維與狀態變數相對應，由於直接觀測的量是位置zk=(x⎯⎯⎯,y⎯⎯⎯)t ，我們只需要取狀態變數的前兩個元素就夠了，所以h設計成如下：
h=[10010000]
用矩陣形式寫開就是
zk=hxk+vk→[x⎯⎯⎯y⎯⎯⎯]k=[10010000]⎡⎣⎢⎢⎢⎢xyvxvy⎤⎦⎥⎥⎥⎥+vk=[xy]k+vk
我再舉乙個簡單例子：
乙個運動目標的狀態變數xk=(x(k),x˙(k),x¨(k))t
,其中第一項是目標在k時刻的位置，這裡假設為一維座標，第二項為k時刻的速度，第三項為k時刻的加速度，雷達僅能觀測到目標的位置即x(k) ,那麼它的狀態轉移矩陣就該設計成：
a=⎡⎣⎢⎢⎢100δt10δt22δt1⎤⎦⎥⎥⎥
它的觀測矩陣就該設計成：
h=[100]
到了這裡怎麼設計矩陣a和h應該有思路了吧。
三、演算法流程
接下來就開始介紹卡爾曼濾波最核心的五個更新方程了
1、**：
x̂ k⎯⎯⎯=ax̂ k−1+buk−1
p̂ k⎯⎯⎯=ap̂ k−1at+q
2、更新：
x̂ k=x̂ k⎯⎯⎯+kk(zk−hx̂ k⎯⎯⎯)
kk=p̂ k⎯⎯⎯hthp̂ k⎯⎯⎯ht+r
p̂ k=(i−kkh)p̂ k⎯⎯⎯
先來解釋一下公式中各個變數的含義：
x̂ k⎯⎯⎯
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%%%%%%%%%%%平滑濾波%%%%%%%%%%%%%%%%%%%%%%%%
filter_wid = 10;
smooth_res = zeros(1,n);
for i = filter_wid + 1 : n
tempsum = 0;
for j = i - filter_wid : i - 1
tempsum = tempsum + y(j);
endsmooth_res(i) = tempsum / filter_wid;
end% figure(1);
% hist(y);
t=1:n;
figure(1);
expvalue = zeros(1,n);
for i = 1: n
expvalue(i) = con;
endplot(t,expvalue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');
legend('real temperature','kalman result','measured value','smooth result');
axis([0 n 20 30])
xlabel('sample time');
ylabel('room temperature');
title('smooth filter vs kalman filter');
卡爾曼濾波的理解以及引數調整
卡爾曼濾波器是一種最優線性狀態估計方法 等價於 在最小均方誤差準則下的最佳線性濾波器 所謂狀態估計就是通過數學方法尋求與觀測資料最佳擬合的狀態向量。在移動機械人導航方面，卡爾曼濾波是最常用的狀態估計方法。直觀上來講，卡爾曼濾波器在這裡起了資料融合的作用，只需要輸入當前的測量值 多個感測器資料 和上乙...
卡爾曼濾波的理解
首先了解一下濾波 濾波 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是測量系統的引數，對於多...