首先，我們引入乙個離散控制過程的系統。該系統可用乙個線性隨機微分方程來描述：
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)式的**方程得到的）
其中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'/(h*p(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是單位矩陣)
我們還是先來看看網上大肆流傳的乙個例子吧。白字是原文，紅字是我的吐槽。
我覺得應該我的標註應該能讓大家看懂這個例子了解整個迭代計算的過程了，下面我們看看程式吧，這個是乙個比較簡單的波形跟隨，希望能給大家一點啟示。
? 1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
%第一條曲線
% 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])
holdon;
%系統方程：x(k+1)=a*x(k)+w(k)
%觀測方程：z(k)=h*z(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'/(h*p_kk(1)*h'+r); %卡爾曼增益 kg
p_k(1)=a*p_kk(1)*a'+q ; %方差** p_k/k-1
fori=2:length(t)
z_k(i)=h*x(i)+w(i) ;
kg(i)=p_k(i-1)*h'/((h*p_k(i-1)*h'+r)) ;
p_k(i)=a*p_kk(i-1)*a'+q;
p_kk(i)=p_k(i-1)-kg(i)*h*p_kk(i-1);
x_k(i)=a*x_k(i-1)+kg(i-1)*(z_k(i)-h*a*x_k(i-1)); %這邊就直接代入式（1），所以沒出現
學習卡爾曼濾波
偶然機會想學學 最起碼知道一下 卡爾曼濾波，以前學人工智慧課程學過濾波演算法，但是老師只是提了一下有這個東西，一直不知道如何用它來解決問題。1，參考資料 http www.cs.unc.edu welch kalman 裡面有乙個 an introduction to the kalman filt...
卡爾曼濾波學習
clear clc f 1 1 0 1 b 0.5 1 h 0 1 x 10 10 初始狀態 u 1 加速度 p 1 0 0 1 初始協方差矩陣 q 0.001 0 0 0.001 z 1 1000 測量值，勻加速運動 noise 15 randn 1,1000 測量雜訊100個 z z noise...
卡爾曼濾波的理解
首先了解一下濾波 濾波 filtering is weighting 濾波即加權 濾波的作用就是給不同的訊號分量不同的權重。最簡單的loss pass filter，就是直接把低頻的訊號給0權重，而給高頻部分1權重。常見的貝葉斯濾波和卡爾曼濾波 貝葉斯濾波我覺得關鍵在於對先驗概率和後驗概率的理解上，...