在我總結kalman filtering之前請允許我發洩一下，網上的各版本的卡爾曼濾波方程的變數字母真是他媽多，而範例卻全都是同乙個測量氣溫的簡單例子，單純看書的話公式自己又推不出來，真是日了狗了。
好了，說到卡爾曼濾波，才華橫溢卡爾曼老先生到去年終於是去世了（此處僅有敬意），而卡爾曼濾波的作用卻日益彰顯。大概我對卡爾曼濾波的初步理解就是（反正這句話也是抄的，看看就好了，我其實也不懂）：根據當前時刻的觀測值、上一時刻的**值及**誤差,計算得到當前的最優量去**下一刻的量。至於對卡爾曼濾波意義理解的話可以在知乎上搜一下，有個測豬體重的例子感覺十分生動。公式推導的話智商過低的本人也是推不出來的，所以在此僅希望幫助大家學會運用，如果幫得上的話。
首先，我們引入乙個離散控制過程的系統。該系統可用乙個線性隨機微分方程來描述：
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是測量系統的引數，對於多測量系統，h為矩陣。w(k)和v(k)分別表示過程和測量的雜訊。他們被假設成高斯白雜訊，他們的協方差分別是q，r。由於系統中一般不太有控制量，所以b這個引數一般為0，也就是沒有u(k)。
以下是程式設計需要的五個卡爾曼濾波的迭代方程：
x(k|k-1)=ax(k-1|k-1)+bu(k)…(1)
其中x(k|k-1)是上一時刻的狀態對現在時刻狀態的**，x(k-1|k-1)是上一時刻狀態的最優結果， u(k)為現在時刻狀態的控制量。
（一開始看我也一臉蒙蔽，主要看一下x(k|k-1)這樣的變數到底代表什麼）
系統的狀態已經更新，現在需要更新系統的誤差估計協方差矩陣，用p(k|k-1)表示誤差估計協方差矩陣：
p(k|k-1)=a*p(k-1|k-1)a』+q…(2)
（這個協方差的由來是由(1)式的**方程得到的，你看他乘的係數也是a就懂了）
其中p(k|k-1)是在k時刻由上一狀態對此狀態的**， p(k-1|k-1)是x(k-1|k-1)對應的誤差估計協方差矩陣，q表示系統過程雜訊的協方差。
現在我們得到了**結果，然後我們根據得到的現在狀態的測量值進行修正得到最優的估計量x(k|k):
x(k|k)=x(k|k-1)+kg(k)*(z(k)-hx(k|k-1))…(3)
（常常在程式設計的時候會直接把（1）式直接代入這個式子，所以你們看不到（1）式）
(3)式中kg(k)未知，則需要對其就行求解，就引出(4)式：
kg(k)=p(k|k-1)h』/(hp(k|k-1)*h』 + r)…(4)
(這個是卡爾曼增益，你只要知道這是用來修正**值的乙個引數就好了)
到現在，我們以及得出的k 時刻的系統狀態的最優值x(k|k),為了讓卡爾曼濾波器不斷地進行下去，我們需要更新 x(k|k) 對應的p(k|k)
p(k|k)=(i-kg(k)*h)*p(k|k-1)…(5)
((3)(4)式計算出來的x(k|k)、p(k|k)又會迭代回(1)（2）式中，注意i是單位矩陣)
我們還是先來看看網上大肆流傳的乙個例子吧。白字是原文，紅字是我的吐槽。
假設我們要研究的物件是乙個房間的溫度。根據你的經驗判斷，這個房間的溫度是恆定的。（這裡的假設相當於狀態方程的係數a為1）假設你對你的經驗不是100%的相信，可能會有上下偏差幾度，我們把這些偏差看成是高斯白雜訊（這裡就是w(k)）。另外，我們在房間裡放乙個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白雜訊。（溫度計的測量值就是z(k),而由於溫度測到的溫度就是溫度，不用再換算，所以係數h就是1，偏差就是v(k)）。好了，現在對於某一分鐘我們有兩個有關於該房間的溫度值：你根據經驗的**值（系統的**值x(k|k-1)）和溫度計的值（測量值z(k)）。下面我們要用這兩個值結合他們各自的雜訊來估算出房間的實際溫度值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k+1時刻，進行新的最優估算。到現在為止，好像還沒看到什麼自回歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56 度）的偏差。演算法如下：((1-kg)*52)0.5=2.35（該式對應（5）式）。這裡的5就是上面的k時刻你**的那個23度溫度值的偏差，得出的2.35就是進入 k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。就是這樣，卡爾曼濾波器就不斷的把 covariance遞迴，從而估算出最優的溫度值。他執行的很快，而且它只保留了上一時刻的covariance。上面的kg，就是卡爾曼增益（kalman gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！（自學了好久菜看懂這個例子是什麼意思）
我覺得應該我的標註應該能讓大家看懂這個例子了解整個迭代計算的過程了，下面我們看看程式吧，這個是乙個比較簡單的波形跟隨，希望能給大家一點啟示。12
3456
78910
1112
1314
1516
1718
1920
2122
2324
2526
2728
2930
3132
3334
3536
3738
3940
4142
4344
4546
4748
4950
5152
5354
5556
%第一條曲線
% t=0.1:0.1:6;
% x=t.^2;
%第二條曲線
% t=1:1:60;
% x=t.^2;
%第三條曲線
t=1:0.5:60;
x=3*sin(t);
% %第四條曲線
% t=1:0.5:60;
% x=sin(t);
%n=1:60;
figure;
subplot(2,1,1);
plot(1:0.5:60,x,『r』);
axis([0 60 -5 5])
hold on;
%系統方程：x(k+1)=ax(k)+w(k)
%觀測方程：z(k)=hz(k)+v(k)
%這個是生成高斯雜訊的隨機數
w=randn(1,length(t));
v=randn(1,length(t));
a=1;
h=1;
x_k(1)=0; %狀態估計初值
p_kk(1)=0; %p（k/k）
p_k(1)=0; %p（k/k-1） 這個初始化不需要，就給你們看看變數的對應
z_k(1)=x_k(1)+w(1) ; %測量值
r=(std(v)).^2;
q=(std(w)).^2;
kg(1)=p_kk(1)h』/(hp_kk(1)h』+r); %卡爾曼增益 kg
p_k(1)=ap_kk(1)*a』+q ; %方差** p_k/k-1
for i=2:length(t)
z_k(i)=hx(i)+w(i) ;
kg(i)=p_k(i-1)h』/((hp_k(i-1)h』+r)) ;
p_k(i)=ap_kk(i-1)a』+q;
p_kk(i)=p_k(i-1)-kg(i)hp_kk(i-1);
x_k(i)=ax_k(i-1)+kg(i-1)(z_k(i)-hax_k(i-1)); %這邊就直接代入式（1），所以沒出現
endn=1:60;
plot(1:0.5:60,x_k);
subplot(2,1,2);
plot(1:0.5:60,z_k);
axis([0 60 -5 5])
後續可能還有關於不同訊號型別的卡爾曼濾波分析
卡爾曼 卡爾曼濾波 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...