轉自：
https://www.jianshu.com/p/d3b1c3d307e0
卡爾曼濾波在我當學生的時候就用過，但是當年我似乎就是套公式，沒有理解其精髓，加之時間久了有點模糊，突然需要指導學生使用，有了強烈的陌生感覺，不得不逼自己再一次撿起。自己學會和教會別人是學習的兩個層次，爲了自我提高，也爲了更好得指導學生。於是，我又翻出自己當年寫的算法以及在網上找了些大神寫的資料，進行融會貫通，總結提煉，希望稍微有點大學概率論的人能夠看懂此文並熟練使用。
爲了可以更加容易得理解卡爾曼濾波器，我們先回顧了基礎的數學知識，然後採用形象的描述方法來講解其算法本質，接着利用數學符號抽象出數學公式並進行歸納總結，最後給出計算機程序實現。希望初學者從頭看到尾，循序漸進，具有較好數學功底者可直接看數學公式，實用主義者可直接拷貝源代碼。
1．卡爾曼與其提出的濾波器
在學習卡爾曼濾波器之前，先搞懂“卡爾曼”是什麼鬼？其實卡爾曼是人，而且還是一個現代的牛人！卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電機工程學士及碩士學位。1957年於哥倫比亞大學獲得博士學位。
我們現在要學習的卡爾曼濾波器，正是源於他的博士論文和1960年發表的論文《A New Approach to Linear Filtering and Prediction Problems》(線性濾波與預測問題的新方法)。寫到這我默默流淚，爲啥我的論文寫完後連自己都不想在看呢？
簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優化自迴歸數據處理算法）”。對於解決很大部分的問題，他是最優，效率最高甚至是最有用的。他的廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機圖像處理，例如人臉識別，圖像分割，圖像邊緣檢測等等。
2.基礎數學知識準備
在正式開講前，我們需要回顧下數學基礎知識，準確的說是概率論的知識。
概率論裏，每一個隨機變量X都是具有一個平均值（這個值在變量的概率密度函數分佈圖的最中心位置，代表該數值是最可能發生的）和方差（這個數值代表變量的不確定性程度）。平均值μ就是數學期望μ=E(X)。方差D(X)=σ2=E{[x-E(x)]2}是衡量隨機變量或一組數據時離散程度的度量。方差的平方根即標準差或者均方差，即σ。
那麼，一談到概率統計，我們馬上可以想到：兩個變量是否相互獨立，或者互不相關。相互獨立是已知其中一個變量的變換規律，我們無法推斷出另外一個變量的變化規律，否者就相互不獨立。例如，根據牛頓公式測量的速度和位移隨機變量就是相關的。那麼相關的程度我們可以用協方差來描述，數學表述爲：
人們總結隨機變量的概率分佈，可得到一些典型的分佈，比如均勻分佈、泊松分佈和高斯分佈等。其中高斯分佈，也稱正態分佈，又稱常態分佈，是我們常見的分佈，記爲N（μ，σ2），其中μ，σ2爲分佈的參數，分別爲高斯分佈的期望和方差。當有這兩者確定值時，概率p(x)也就確定了，特別當μ=0，σ^2=1時，X的分佈爲標準正態分佈。
我們考察單隨機變量的高斯分佈，其概率密度函數爲：
如果存在兩個高斯分佈，當兩個分佈相乘時，得到什麼結果？（推導過程感興趣的童鞋可以看隨機過程一書）：
結論就是兩正態分佈的乘積仍然是正態分佈，且新正態分佈的均值和方差如下：
以上是單變量概率密度函數的計算結果，如果是多變量的，那麼，就變成了協方差矩陣的形式：
3.卡爾曼濾波的通俗理解
講解了隨機變量和概率，那麼爲啥要引入卡爾曼濾波呢。卡爾曼濾波適用於估計一個由隨機變量組成的動態系統的最優狀態。即便是觀測到的系統狀態參數含有噪聲，觀測值不準確，卡爾曼濾波也能夠完成對狀態真實值的最優估計。
爲了更好理解爲啥需要卡爾曼濾波，我們還是舉個實際的例子說明。
考慮軌道上的一個小車，無外力作用。爲了便於對它進行控制，需要知道它的位置以及移動速度。所以，建立一個向量，用來存儲小車的位置和速度：x(t)=(p,v)，此時位置和速度兩者都爲隨機變量，才爲二維隨機向量，後面舉例會簡化爲一維，只有位置是隨機變量。一個系統的狀態有很多，選擇最關心的狀態來建立這個狀態向量是很重要的。例如，狀態還有水庫裏面水位的高低、鍊鋼廠高爐內的溫度、平板電腦上面指尖觸碰屏幕的位置等等這些需要持續跟蹤的物理量。對於小車我們就採用位置和速度來描述，它在時刻t的狀態向量x(t)只與x(t-1)相關：比如：x(0)表示小車在t=0時刻的狀態是：速度大小5m/s,速度方向右,位置座標0。反正有了這個向量就可以完全預測t=1時刻小車的狀態。
因爲X(t)=Fx(t-1)，那麼根據t=0時刻的初值x(0)，理論上我們可以求出它任意時刻的狀態。當然，實際情況不會這麼美好。這個遞推函數可能會受到各種不確定因素的影響（內在的，小車結構不緊密，輪子不圓等，外在的，比如颳風，下雨，地震等）導致x(t)並不能精確標識小車實際的狀態。我們假設每個狀態分量受到的不確定因素都服從正態分佈。
如上所述，狀態向量是有維度的。其實如果是一維的卡爾曼濾波很容易說清楚，不比最小二乘法複雜多少。如果涉及多維就沒有那麼容易理解，因爲被抽象爲矩陣的形式。對，就是矩陣，一個和線性代數相關的概率，如需要了解多維卡爾曼濾波，請大家自行腦補。
我們首先看最簡單的一維情況：假設小車速度均值爲2cm/s，我們只用位置表示小車的狀態，即只對小車位置進行估計。此時把二維向量簡化爲標量。
請看下圖：t=k-1時小車的位置服從紅色的正態分佈。假設位置在21cm，位置誤差爲0.3cm，即μ=21，σ=0.3。
根據小車的這個位置，我們可以根據位置與速度經驗公式，預測出t=k時刻它的位置：位置均值是21+2=23cm，我對自己速度預測不確定度爲0.4cm，那麼此時小車位置預測誤差爲0.5cm（0.5是這樣得到的：如果k-1時刻估算出的最優位置的偏差是0.3，你對自己預測的不確定度是0.4，他們平方相加再開方，就是0.5，爲什麼是這樣？用了正態分佈的線性疊加定理，即位置和速度兩正態分佈的和也滿足正態分佈，方差是二者平方 ）。可以看出0.5>0.3，此時分佈變“胖”了，這很好理解——因爲在遞推的過程中又加了一層噪聲（即預測不穩定度），所以不確定度變大了。
爲了避免純估計帶來的偏差，我們在t=k時刻對小車的位置座標進行一次雷達測量，當然雷達對小車距離的測量也會受到種種因素的影響，會帶來測量誤差，這個測量誤差的均方差是0.4cm，假設此時測量值是25cm(即均值爲25)。而且測量結果也複合正態分佈，於是可以畫出小車在t=1時的位置服從藍色分佈。
好了，現在我們得到兩個不同的結果。那麼哪個纔是更加可靠的小車現在的位置呢？有人提過加權，那麼怎麼加權才科學呢？
大家讀到這裏心裏非常清楚的一點是：兩個事件的發生都是概率性的，不能完全相信其中的任何一個！如果我們具有兩個事件，從直覺或者是理性思維上講，是不是認定兩個事件都發生，就找到了那個最理想的估計值？好了，抽象一下，得到：兩個事件同時發生的可能性越大，我們越相信它！要想考察它們同時發生的可能性，就是將兩個事件單獨發生的概率相乘。於是乎，卡爾曼濾波的思想誕生了！Kalman老先生的牛逼之處就在於找到了相應權值，使紅藍分佈合併爲下圖這個綠色的正態分佈。
你問我kalman爲什麼牛逼？那是因爲人家在理論上證明了：綠色分佈不僅保證了在紅藍給定的條件下，小車位於該點的概率最大，而且，它居然還是一個正態分佈！正態分佈就意味着，可以把它當做初值繼續往下算了！這是Kalman濾波能夠迭代的關鍵。
由於我們用於估算k時刻的實際位置有兩個位置值，分別是23cm和25cm。究竟實際位置是多少呢？相信自己預測還是相信雷達測量呢？究竟相信誰多一點，我們可以用kalman的方法來加權，即利用他們的方差σ^2來判斷，求出綠色分佈均值位置在紅藍均值間的比例，即Kalman增益Kg。
根據數學上兩正態分佈相乘仍然是正態分佈，由公式(3)得到Kg=0.52/(0.52+0.4^2)，所以Kg=0.61。（注意kg是方差的概念，不是均方差，網上很多文獻kg用錯，開了根號，得到0.78）我們可以估算出k時刻的實際位置是：23+0.61*(25-23)=24.22cm，見公式(1)。可以看出，因爲雷達的方差比較小（比較相信雷達），所以估算出的最優位置值偏向雷達測量的值。
現在我們已經得到k時刻的最優位置值了，下一步就是要進入k+1時刻，進行新的最優估算。到現在爲止，好像還沒看到什麼自迴歸的東西出現。對了，在進入k+1時刻之前，我們還要算出k時刻那個最優值（24.22cm）的偏差。算法如下：((1-Kg)*0.52)0.5=0.312，見公式(2)。這裏的0.5就是上面的k時刻你預測的那個23cm位置的均方差，得出的0.312cm就是進入k+1時刻以後k時刻估算出的最優位置值的均方差（對應於上面的0.3cm），於是我們可以把綠色分佈當做第一張圖中的紅色分佈對t=k+1時刻進行預測，算法就可以開始循環往復了。
說明一下，由於上面我們只對小車位移這個一維量（向量）做了估計，因此Kalman增益是標量。如果狀態向量是多維，那麼方差就該變爲協方差，從而引入協方差矩陣的迭代，Kalman增益也變成了矩陣。需要採用公式(4)-(6)來求解Kalman增益矩陣和k時刻的最優值和協方差矩陣。無論是多少維的向量空間，上面描述給出了Kalman濾波算法的本質就是利用兩個正態分佈的融合仍是正態分佈這一特性進行迭代而已。
4.卡爾曼濾波的嚴格數學描述
有了上述的理解，下面就要言歸正傳，討論真正工程系統上的卡爾曼濾波。
首先，我們先要引入一個離散控制過程的系統。該系統可用一個線性隨機微分方程（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（這裏我們假設他們不隨系統狀態變化而變化）。
對於滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的信息處理器。下面我們結合他們的協方差來估算系統的最優化輸出（類似上一節小車一維的例子，現在擴展到多維）。
首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是k，根據系統的模型，可以基於系統的上一狀態而預測出現在狀態：
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (7)
式(7)中，X(k|k-1)是利用上一狀態預測的結果，X(k-1|k-1)是上一狀態最優的結果，U(k)爲現在狀態的控制量，如果沒有控制量，它可以爲0。
到現在爲止，我們的系統結果已經更新了，可是，對應於X(k|k-1)的協方差還沒更新。我們用P表示協方差(covariance)：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (8)
式(8)中，P(k|k-1)是X(k|k-1)對應的協方差，P(k-1|k-1)是X(k-1|k-1)對應的協方差，A’表示A的轉置矩陣，Q是系統過程的協方差。式子7，8就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。
現在我們有了現在狀態的預測結果，然後我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態(k)的最優化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))……… (9)
其中Kg爲卡爾曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)……… (10)
到現在爲止，我們已經得到了k狀態下最優的估算值X(k|k)。但是爲了要令卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新k狀態下X(k|k)的協方差：
P(k|k)=（I-Kg(k) H）P(k|k-1) ………(11)
其中I爲1的矩陣，對於單模型單測量，I=1。當系統進入k+1狀態時，P(k|k)就是式子(8)的P(k-1|k-1)。這樣，算法就可以自迴歸的運算下去。
5.卡爾曼濾波的編程實現
卡爾曼濾波器的原理基本描述了，式子8，9，10，11和12就是他的5個基本公式。根據這5個公式，可以很容易用計算機編程實現，具體思想如下：
假設如下一個系統：
房間內連續兩個時刻溫度差值的標準差爲0.02度，溫度計的測量值誤差的標準差爲0.5度，房間溫度的真實值爲24度，對溫度的初始估計值爲23.5度，誤差的方差爲1。
MatLab仿真的代碼如下：
% Kalman filter example of temperaturemeasurement in Matlab
% This M code is modified from Xuchen Yao’smatlab on 2013/4/18
%房間當前溫度真實值爲24度，認爲下一時刻與當前時刻溫度相同，誤差爲0.02度（即認爲連續的兩個時刻最多變化0.02度）。
%溫度計的測量誤差爲0.5度。
%開始時，房間溫度的估計爲23.5度，誤差爲1度。
clear all;
close all;
% intial parameters
n_iter = 100; %計算連續n_iter個時刻
sz = [n_iter, 1]; % size of array. n_iter行，1列
x = 24; %溫度的真實值
Q = 4e-4; %過程方差，反應連續兩個時刻溫度方差。更改查看效果
R = 0.25; %測量方差，反應溫度計的測量精度。更改查看效果
z = x + sqrt®*randn(sz); % z是溫度計的測量結果，在真實值的基礎上加上了方差爲0.25的高斯噪聲。
%對數組進行初始化
xhat=zeros(sz); %對溫度的後驗估計。即在k時刻，結合溫度計當前測量值與k-1時刻先驗估計，得到的最終估計值
P=zeros(sz); %後驗估計的方差
xhatminus=zeros(sz); %溫度的先驗估計。即在k-1時刻，對k時刻溫度做出的估計
Pminus=zeros(sz); %先驗估計的方差
K=zeros(sz); %卡爾曼增益，反應了溫度計測量結果與過程模型（即當前時刻與下一時刻溫度相同這一模型）的可信程度
% intial guesses
xhat(1) = 23.5; %溫度初始估計值爲23.5度
P(1) =1; %誤差方差爲1
for k = 2:n_iter
%時間更新（預測）
xhatminus(k) = xhat(k-1); %用上一時刻的最優估計值來作爲對當前時刻的溫度的預測
Pminus(k) = P(k-1)+Q; %預測的方差爲上一時刻溫度最優估計值的方差與過程方差之和
%測量更新（校正）
K(k) = Pminus(k)/( Pminus(k)+R ); %計算卡爾曼增益
xhat(k) =
xhatminus(k)+K(k)*(z(k)-xhatminus(k)); %結合當前時刻溫度計的測量值，對上一時刻的預測進行校正，得到校正後的最優估計。該估計具有最小均方差
P(k) = (1-K(k))*Pminus(k); %計算最終估計值的方差
end
FontSize=14;
LineWidth=3;
figure();
plot(z,‘k+’); %畫出溫度計的測量值
hold on;
plot(xhat,‘b-’,‘LineWidth’,LineWidth) %畫出最優估計值
hold on;
plot(x*ones(sz),‘g-’,‘LineWidth’,LineWidth);
%畫出真實值
legend(‘溫度計的測量結果’, ‘後驗估計’, ‘真實值’);
xl=xlabel(‘時間(分鐘)’);
yl=ylabel(‘溫度’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
hold off;
set(gca,‘FontSize’,FontSize);
figure();
valid_iter = [2:n_iter]; % Pminus not validat step 1
plot(valid_iter,P([valid_iter]),‘LineWidth’,LineWidth);
%畫出最優估計值的方差
legend(‘後驗估計的誤差估計’);
xl=xlabel(‘時間(分鐘)’);
yl=ylabel(‘℃^2’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
set(gca,‘FontSize’,FontSize);
計算結果如圖所示：
另外，跨平臺計算機視覺庫Opencv目標版本中帶有kalman這個類，可以使用它來完成一些跟蹤目的。下面來看看使用Kalman編程的主要步驟：
步驟一：
Kalman這個類需要初始化下面變量：
轉移矩陣，測量矩陣，控制向量(沒有的話，就是0)，過程噪聲協方差矩陣，測量噪聲協方差矩陣，後驗錯誤協方差矩陣，前一狀態校正後的值，當前觀察值。
步驟二：
調用kalman這個類的predict方法得到狀態的預測值矩陣，預測狀態的計算公式如下：
predicted state (x’(k)):x’(k)=Ax(k-1)+Bu(k)
其中x(k-1)爲前一狀態的校正值，第一個循環中在初始化過程中已經給定了，後面的循環中Kalman這個類內部會計算。A,B,u(k),也都是給定了的值。這樣進過計算就得到了系統狀態的預測值x’(k)了。
步驟三：
調用kalman這個類的correct方法得到加入觀察值校正後的狀態變量值矩陣，其公式爲：
corrected state (x(k)):x(k)=x’(k)+K(k)(z(k)-Hx’(k))
其中x’(k)爲步驟二算出的結果，z(k)爲當前測量值，是我們外部測量後輸入的向量。H爲Kalman類初始化給定的測量矩陣。K(k)爲Kalman增益，其計算公式爲：
Kalman gain matrix (K(k)): K(k)=P’(k)Htinv(H*P’(k)*Ht+R)
計算該增益所依賴的變量要麼初始化中給定，要麼在kalman理論中通過其它公式可以計算。
經過步驟三後，我們又重新獲得了這一時刻的校正值，後面就不斷循環步驟二和步驟三即可完成Kalman濾波過程。