（1）
簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自迴歸數據處理算法）”。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。
匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預測問題的新方法）。如果對這編論文有興趣，可以到這裏的地址下載： http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
NASA埃姆斯研究中心訪問時,發現他的方法對於解決阿波羅計劃的軌道預測很有用,後來阿波羅飛船的導航電腦使用了這種濾波器. 關於這種濾波器的論文由Swerling (1958), Kalman (1960)與 Kalman and Bucy (1961)發表.
目前,卡爾曼濾波已經有很多不同的實現.卡爾曼最初提出的形式現在一般稱爲簡單卡爾曼濾波器.除此以外,還有施密特擴展濾波器,信息濾波器以及很多Bierman, Thornton 開發的平方根濾波器的變種.也行最常見的卡爾曼濾波器是鎖相環，它在收音機,計算機和幾乎任何視頻或通訊設備中廣泛存在.
卡爾曼濾波的一個典型實例是從一組有限的，對物體位置的，包含噪聲的觀察序列預測出物體的座標位置及速度. 在很多工程應用(雷達, 計算機視覺)中都可以找到它的身影. 同時，卡爾曼濾波也是控制理論以及控制系統工程中的一個重要話題.
比如,在雷達中,人們感興趣的是跟蹤目標,但目標的位置,速度,加速度的測量值往往在任何時候都有噪聲.卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響,得到一個關於目標位置的好的估計。這個估計可以是對當前目標位置的估計(濾波),也可以是對於將來位置的估計(預測)，也可以是對過去位置的估計(插值或平滑).
高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前後時間是沒有關係的而且符合高斯分配（Gaussian Distribution）。另外，我們在房間裏放一個溫度計，但是這個溫度計也不準確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。
好了，現在對於某一分鐘我們有兩個有關於該房間的溫度值：你根據經驗的預測值（系統的預測值）和溫度計的值（測量值）。下面我們要用這兩個值結合他們各自的噪聲來估算出房間的實際溫度值。
假如我們要估算k時刻的是實際溫度值。首先你要根據k-1時刻的溫度值，來預測k時刻的溫度。因爲你相信溫度是恆定的，所以你會得到k時刻的溫度預測值是跟k-1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優溫度值的偏差是3，你對自己預測的不確定度是4度，他們平方相加再開方，就是5）。然後，你從溫度計那裏得到了k時刻的溫度值，假設是25度，同時該值的偏差是4度。
由於我們用於估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的協方差（covariance）來判斷。因爲Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因爲溫度計的covariance比較小（比較相信溫度計），所以估算出的最優溫度值偏向溫度計的值。
現在我們已經得到k時刻的最優溫度值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在爲止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這裏的5就是上面的k時刻你預測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以後k時刻估算出的最優溫度值的偏差（對應於上面的3）。
就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的covariance。上面的Kg，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值，是不是很神奇！
概率（Probability），隨機變量（Random Variable），高斯或正態分配（Gaussian Distribution）還有State-space Model等等。但對於卡爾曼濾波器的詳細證明，這裏不能一一描述。
首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（Linear Stochastic Difference equation）來描述：
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（這裏我們假設他們不隨系統狀態變化而變化）。
對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結合他們的covariances 來估算系統的最優化輸出（類似上一節那個溫度的例子）。
首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：
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)。這樣，算法就可以自迴歸的運算下去。
卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個基本公式。根據這5個公式，可以很容易的實現計算機的程序。
下面，我會用程序舉一個實際運行的例子。
這裏我們結合第二第三節，舉一個非常簡單的例子來說明卡爾曼濾波器的工作過程。所舉的例子是進一步描述第二節的例子，而且還會配以程序模擬結果。
根據第二節的描述，把房間看成一個系統，然後對這個系統建模。當然，我們見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因爲測量的值是溫度計的，跟溫度直接對應，所以H=1。式子3，4，5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)
現在我們模擬一組測量值作爲輸入。假設房間的真實溫度爲25度，我模擬了200個測量值，這些測量值的平均值爲25度，但是加入了標準偏差爲幾度的高斯白噪聲（在圖中爲藍線）。
爲了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個就可以了，因爲隨着卡爾曼的工作，X會逐漸的收斂。但是對於P，一般不要取0，因爲這樣可能會令卡爾曼完全相信你給定的X(0|0)是系統最優的，從而使算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。
該系統的真實溫度爲25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優化結果（該結果在算法中設置了Q=1e-6，R=1e-1）。
附matlab下面的kalman濾波程序：
clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;
p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');
function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)
% Kalman filter.
% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)
%
% INPUTS:
% y(:,t) - the observation at time t
% A - the system matrix
% C - the observation matrix
% Q - the system covariance
% R - the observation covariance
% init_x - the initial state (column) vector
% init_V - the initial state covariance
%
% OPTIONAL INPUTS (string/value pairs [default in brackets])
% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]
% In this case, all the above matrices take an additional final dimension,
% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).
% However, init_x and init_V are independent of model(1).
% 'u' - u(:,t) the control signal at time t [ [] ]
% 'B' - B(:,:,m) the input regression matrix for model m
%
% OUTPUTS (where X is the hidden state being estimated)
% x(:,t) = E[X(:,t) | y(:,1:t)]
% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]
% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2
% loglik = sum{t=1}^T log P(y(:,t))
%
% If an input signal is specified, we also condition on it:
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]
% If a model sequence is specified, we also condition on it:
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]
[os T] = size(y);
ss = size(A,1); % size of state space
% set default params
model = ones(1,T);
u = [];
B = [];
ndx = [];
args = varargin;
nargs = length(args);
for i=1:2:nargs
switch args
case 'model', model = args{i+1};
case 'u', u = args{i+1};
case 'B', B = args{i+1};
case 'ndx', ndx = args{i+1};
otherwise, error(['unrecognized argument ' args])
end
end
x = zeros(ss, T);
V = zeros(ss, ss, T);
VV = zeros(ss, ss, T);
loglik = 0;
for t=1:T
m = model(t);
if t==1
%prevx = init_x(:,m);
%prevV = init_V(:,:,m);
prevx = init_x;
prevV = init_V;
initial = 1;
else
prevx = x(:,t-1);
prevV = V(:,:,t-1);
initial = 0;
end
if isempty(u)
[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);
else
if isempty(ndx)
[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...
kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...
'initial', initial, 'u', u(:,t), 'B', B(:,:,m));
else
i = ndx;
% copy over all elements; only some will get updated
x(:,t) = prevx;
prevP = inv(prevV);
prevPsmall = prevP(i,i);
prevVsmall = inv(prevPsmall);
[x(i,t), smallV, LL, VV(i,i,t)] = ...
kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...
'initial', initial, 'u', u(:,t), 'B', B(i,:,m));
smallP = inv(smallV);
prevP(i,i) = smallP;
V(:,:,t) = inv(prevP);
end
end
loglik = loglik + LL;
end
轉載自：http://bbs.sciencenet.cn/home.php?mod=space&uid=433380&do=blog&id=336646
（2）
還有下面一個 Matlab源程序，顯示效果更好。
clear
clc;
N=300;
CON = 25;%房間溫度，假定溫度是恆定的
%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%
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;%對應於預估值的協方差
kg = p / (p + R);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
end
%%%%%%%%%%%Smoothness Filter%%%%%%%%%%%%%%%%%%%%%%%%
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);
end
smooth_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;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');
legend('expected','estimate','measure','smooth result');
axis([0 N 20 30])
xlabel('Sample time');
ylabel('Room Temperature');
title('Smooth filter VS kalman filter');