假設我們要測量房間的溫度，我們可以用溫度計測量，由于溫度計測量精度不高，有±3度的偏差。我們對房間溫度進行了10秒鐘的測量，每隔1秒測量一次，假設10次資料分别為：
溫度測量資料
25.3度
26.5度
24.0度
（溫度計開始受到幹擾）
35.0度
（幹擾消失）
22.9度
25.6度
23.5度
（開啟暖氣）
28.8度
30.2度
29.5度
通常房間的溫度是比較恒定的，而由于溫度計的測量誤差和裝置幹擾，導緻資料在劇烈跳變。如果這種資料給使用者，使用者就會給差評了。
因而，由于測量裝置一方面有誤差，另一方面會引入幹擾，工程上通常不能直接調用測量裝置的資料，要将資料進行濾波優化再調用。
均值濾波
我們最熟悉的濾波是均值濾波，先采樣N次，将N次的值累加起來，再除以N，就可以得到均值。
假設我們對上述溫度測量過程進行均值濾波，N取5次。按照均值濾波算法，得到的溫度是：
T1
=(25.3+26.5+24.0+35.0+22.9)/5
=26.7度
T2
=(25.6+23.5+28.8+30.2+29.5)/5
=27.5度
以看出經過均值濾波後的資料不再跳變得那麼厲害，但是5秒才重新整理一次顯然太慢了，而且後來房間開了暖氣，這個變化也被均值濾波算法淹沒掉了。
一階滞後濾波
為了提高重新整理速率，同時避免淹沒一些真實的變化，我們換用一階滞後濾波算法（真裝逼）。
一階滞後濾波算法公式
a*本次采樣值 + (1-a)*上次濾波結果
↑
其中a可以了解為信任比例,如果a取0.5，則代表信任本次采樣結果占50%的比例，同時相信上次濾波結果也占50%的比例。
由于第1秒時沒有上次濾波結果，我們先假定上次濾波結果為26.0度，權重a為0.6，則依據一階滞後濾波算法可得：
一階滞後濾波處理的溫度資料
25.3*0.6+26.0*0.4=25.6度
26.5*0.6+25.6*0.4=26.1度
24.0*0.6+26.1*0.4=24.8度
35.0*0.6+24.8*0.4=30.9度
22.9*0.6+30.9*0.4=26.1度
25.6*0.6+26.1*0.4=25.8度
23.5*0.6+25.8*0.4=24.4度
28.8*0.6+24.4*0.4=27.0度
30.2*0.6+27.0*0.4=28.9度
29.5*0.6+25.6*0.4=29.3度
以上一階滞後濾波算法的結果既減緩了溫度的跳變，又提高了重新整理速率。但是它仍然沒能排除掉35度的那個幹擾，同時在房間開了暖氣後溫度反應也有點滞後。
問？
那麼是否有什麼算法既可以避開誇張的突變，又可以快速響應合理的突變，還可以做到實時高效地重新整理呢？
卡爾曼濾波
以上問題時有答案的，那就是偉大的卡爾曼濾波
↑
前面的一階滞後濾波算法有一個權重a，這個a在上述計算過程中是不變的，而卡爾曼濾波算法的核心靈魂在于這個a可以根據測量值的變化作實時的更新。
使用卡爾曼濾波前有幾個初始條件需要知道
01
原始值
卡爾曼濾波需要一個原始值給它做第一次疊代用，通常可以用傳感器先測量N組資料，然後求取平均值，将這個平均值當做原始值。本文測室溫的例子可以先定初始值26.0度。
02
測量系統誤差
前面已經假定溫度計的測量誤差是3度。
03
下一次的最大偏內插補點
由于房間溫度相對是恒定的，即使開啟暖氣下一次最多也隻能變化5度，則這個最大偏內插補點就是5度。
04
原始值誤差
由于原始值是通過N次測量求平均得到的，這裡面也會有些誤差，假定原始值誤差是2度。
我們開始應用卡爾曼濾波算法進行疊代。首先，由于我們相信房間溫度恒定，下一秒溫度和這一秒溫度是一樣的。我們用原始值26.0度作為起始溫度，則下一秒的估計溫度也是26.0度，而下一秒的溫度計測量溫度是25.3度。那麼此時我們有兩個可能的溫度值：26.0度和25.3度，那究竟該信誰多一點呢？即它們的信任比例是多少？我們将測量值的信任比例定義為kg，kg符合這個協方差算式：
kg^2
=(原始值誤差^2+下一次的最大偏內插補點^2) /
(原始值誤差^2+下一次的最大偏內插補點^2+測量系統誤差^2)
代入本例中有
kg^2=(2^2+5^2)/(2^2+5^2+3^2)=0.763
開根号得kg=0.874
即測量值25.3度的信任比例是0.874，則第1秒的溫度為：
T1
=26+(25.3-26)*0.874=25.4度
這個25.4度将會變為下一輪疊代計算的原始值，因為根據卡爾曼濾波後的值更加接近真實世界的值了，是以下一輪原始值誤差會變化。
下一輪原始值誤差此時變為：
((1-Kg)*(本輪原始值誤差^2+下一次的最大偏內插補點 ^2))^0.5
=((1-Kg)*(2^2+5^2))^0.5
=1.9
既然原始值的誤差變了，因為kg^2=(原始值誤差^2+下一次的最大偏內插補點^2)/(原始值誤差^2+下一次的最大偏內插補點^2+測量系統誤差^2)，是以kg值也會變化，即信任比例發生變化。
然後開始第二輪疊代計算，第二輪的kg^2=(1.9^2+5^2)/(1.9^2+5^2+3^2)=0.760,即kg=0.872,則第2秒的溫度為：
T2
=25.4+(26.5-25.4)*0.872=26.4度
之後開始第3輪的疊代
第4輪
第5輪
……
第n輪
卡爾曼濾波經過多次的疊代後會逐漸縮小原始值誤差，輸出結果也更接近真實的值。對于測量系統回報的突變值會大幅降低信任比例，而如果測量系統連續兩次輸出的突變值很接近，則卡爾曼濾波會認為這個時候環境真的變化了，立馬又提高信任比例，使得輸出值對真實環境變化的響應很迅速。
如果使用者改變下一次的最大偏內插補點，假設将本例中的最大偏內插補點5度改為2度，那麼測量系統測量值的信任比例将會變小，你将會看到經過濾波輸出的溫度值跳動很小，對幹擾的抑制更加明顯，但是對于真實環境改變的響應也會變得更加滞後。
篇文章《卡爾曼濾波（上）》裡面有兩個公式，可能會比較難以了解一點。因而上篇文章就隻把這兩個公式當做定理去使用，沒有做過多說明。由于本篇文章屬于進階說明，需要對此做一個簡單易懂的講解。
兩個公式
接下來我們對這兩個公式的來由做簡單形象的推導，需要應用到一些機率論知識。
我們還是以房間溫度為例，假設溫度計測量到此刻房間溫度為μ ℃，溫度誤差的方差為σ^2，則此刻真實世界的溫度機率符合正态分布，如下圖所示：
正态分布
在網上随便找的圖，資料沒有意義，核心内容了解即可。
正态分布的數學模型如下：
由于下一個時刻的房間溫度有兩個來源，一個是我們根據此刻房間溫度估計出來的，還有一個是下一時刻溫度計測量到的。不管是估計值還是實測值都有誤差，這兩個值也都符合正态分布，如下圖所示：
估計正态
測量正态
在網上随便找的圖，資料沒有意義，核心内容了解即可。
于是就有一個問題，估計溫度和測量溫度的正态分布究竟信任誰多一點？這裡我們就需要對兩個正态分布進行融合，求取融合後溫度機率分布。
兩個獨立正态分布融合
計算過程比較複雜，具體請參考連結中的論文。
注：論文是PDF格式
連結：http://www.tina-vision.net/docs/memos/2003-003.pdf
總
之兩個獨立正态分布函數最終會融合出一個新的正态分布函數，我們将新的正态分布函數定義為：
μ_算 就是真實世界最有可能的溫度
σ_算^2 就是這個溫度誤差的方差
用圖形表示會形象點：
分布融合
毀圖秀秀畫的，不精确，核心内容了解即可。
我
們求取μ_算和σ_算，依據論文中的計算結果，有：
上面兩式都有這一坨：
大坨的東西看得不舒服，我們用kg表示它:
這個kg通俗點講就是上篇文章提到的信任比例，不俗得講就叫卡爾曼增益。簡化後有：
這兩個式子和文章開頭兩個式子一毛一樣有木有！
下
卡爾曼濾波有五個最基本的公式，我們将對其逐一解讀。
以上的公式看着很吓人，但我将會對其進行通俗講解，看完會有滿滿收獲，是以請别跑開。
前兩篇文章都隻是講如何用卡爾曼濾波對單種資料進行濾波處理，然而工程上往往有多種資料需要同時濾波處理，比如加速度傳感器可以測量3個次元的加速度資訊，這就有3種信号需要做濾波處理。假設我們要對n種資料做卡爾曼濾波，我們應該怎麼做呢？
們可以用單種信号的卡爾曼濾波方程，連續寫n次，然後求取每種信号各自的濾波值。但是這麼多個方程寫出來會很龐大，是以我們就用矩陣的方式來描述，矩陣可以将n個方程組整合成一個方程，如下圖所示：
圖中n個方程組最終化簡成一個矩陣表達式：AX=B，矩陣表示是不是很簡潔？卡爾曼濾波的五個基本公式中的X,A,B,U,P,Q,Kg,H,R,Z,I就都是矩陣，即五個公式其實是五個方程組。
這裡還需要知道一個概念叫向量，向量是一維的矩陣，如下圖所示：
卡爾曼濾波的五個公式中的X,U,Z都是一維矩陣，也就是向量。
最後還需要知道一個概念叫做矩陣轉置，如下圖所示：
至此我們擁有了可以了解卡爾曼濾波最基礎的條件了，我們開始進入正題。
……
正
題
……
我們開始了解第一個公式：
X(k│k-1)=AX(k-1│k-1)+BU(k-1)
假設你要對飛機的歐拉角進行卡爾曼濾波，你可以用傳感器先擷取到一個歐拉角和角速度的初始值，并把這個值當做上一時刻(k-1)的值，然後根據上一時刻(k-1)的值你可以估計出這一時刻(k)飛機的歐拉角為：
Pitch(k)=Pitch(k-1)+GyroY(k-1)*dt
Roll(k)=Roll(k-1)+GyroX(k-1)*dt
Yaw(k)=Yaw(k-1)+GyroZ(k-1)*dt
補充說明
Pitch是俯仰角，GyroY是俯仰角對應的角速度，dt是k-1到k的時間。
Roll是滾轉角，GyroX是滾轉角對應的角速度，dt是k-1到k的時間。
Yaw是偏航角，GyroZ是偏航角對應的角速度，dt是k-1到k的時間。
想辦法将上面的方程組簡化成矩陣表示：
設:
k|k-1代表這一時刻根據上一時刻的估計值
k-1|k-1代表上一時刻卡爾曼處理得到的最優值
k-1代表上一時刻實測值
補充說明
這裡由于才開始第一輪的濾波準備，沒有上一時刻卡爾曼處理得到的最優值，就将k-1時刻測量到的值直接當做最優值。
方程組轉換矩陣
則上面三個方程組就可以表示為：
X(k│k-1)=AX(k-1│k-1)+BU(k-1)
請注意這裡A與B不一定要是對角線才有值的矩陣，隻是在這個簡單例子中是這樣的。
補充說明X(k│k-1)就是指根據上一時刻估計出這一時刻的估計值向量。
A是上一時刻狀态轉移到這一時刻狀态的狀态轉移矩陣。
X(k-1│k-1)是上一時刻的最優值向量，如果卡爾曼濾波是第一次執行，就不存在上一時刻處理得到的最優值，隻能推薦将傳感器上次擷取到的值當做最優值。
B是控制量矩陣。
U(k-1)是上一時刻控制量測量值向量。
至此，第一個公式已經了解。
我們開始了解第二個公式：
P(k│k-1)=AP(k-1│k-1) A^T+Q
回到n種資料做卡爾曼濾波的主題。依據k-1時刻的值我們可以得到k時刻的估計值，但是這個估計值和真實世界的值含有兩種誤差，一種誤差是：
k-1時刻真實世界值X(k-1)和
最優值X(K-1|K-1)之間的誤差還有一種誤差是：
控制量及其對時間積分造成的誤差
後者誤差我們用誤差向量W(k-1)表示
補充說明
w1,w2…wn指n種資料各自控制量及其對時間積分造成的誤差。
卡爾曼濾波的本質
減小誤差的方差
我們想辦法先将估計值的這兩種誤差提取出來，可以令真實世界的值為：
X(k)=AX(k-1)+BU(k-1)+W(k-1)
則真實值和估計值之間的誤差為：
對這個誤差求取方差，矩陣的方差公式為：
補充說明其中D代表方差
YT是Y的轉置cov是協方差(不懂可以先百度：-)
則誤差的方差D為:
依據方差公式：
D(X±Y)=D(X)+D(Y)±2cov(X+Y)
可得：
由于：
“k-1時刻真實世界值X(k-1)和最優值X(K-1|K-1)之間的誤差”
與
“由控制量及其對時間積分造成的誤差”
不會互相制約影響，即兩種誤差獨立，則根據協方差元素獨立則協方差為0的定理，可得：
則誤差的方差可進一步推導為：
矩陣有這麼一個性質
應用這個性質我們可得：
設
則誤差的方差可簡化為：
至此，第二個公式已經了解。
特殊聲明：
這回我們要先了解第四個公式，後面會再回頭了解第三個公式，原因後面看着看着就能明白了。
第四個公式是：
X(k│k)=X(k│k-1)+Kg(k)(Z(k)-HX(k|k-1))
在了解第四個公式前，需要拿傳感器先測量下這一時刻的值，設這一時刻傳感器的測量資料向量為Z(k)。由于有些時候X(k)向量中有一些元素是沒法用傳感器直接測量得到的，但是這個元素又可以利用傳感器的其它測量值推算出來，因而我們将這一時刻傳感器的測量資料向量定義為：
補充說明
H矩陣可以修飾X(k)中某些元素不能通過傳感器直接測量得到，通常H矩陣的斜對角元素是由0和1構成的，0就代表對應元素無法用傳感器直接測量，1代表對應元素可以用傳感器直接測量。
V(k)向量代表傳感器測量誤差，比如稱重秤的誤差為10g，這10g就是這個V(k)。
那這第四個公式在邏輯上就很好了解。先用這一時刻傳感器的測量值減去根據上一時刻估計出來的值，得到殘差，然後用一個比例系數矩陣Kg乘上這個殘差，再加上根據上一時刻的估計值，就可以得到這一時刻最有可能的值，也就是最優值。這個最優值既不完全相信傳感器，也不完全相信估計值，隻要控制好比例系數，就能得到最适合的值。
至此，第四個公式已經了解。
我們開始了解第五個公式：
P(k│k)=(I-Kg(k)H)P(k|k-1)
回顧下之前第二個公式中的P(k|k-1)，是代表上一時刻的估計值和此刻真實世界值的誤差的方差。而P(k|k)代表此刻計算得到的最優值和此刻真實世界值的誤差的方差。則有：
P(k│k)=D[X(k)-X(k|k)]
将：
X(k│k)=X(k│k-1)+Kg(k)[Z(k)-HX(k|k-1)]
代入有：
P(k│k)=D{X(k)-[X(k│k-1)+Kg(k)(Z(k)-HX(k│k-1))]}
将
Z(k)=HX(k)+V(k)
代入有：
補充說明
這裡的I是元素全為1的矩陣。
由于Kg(k)V(k)屬于傳感器測量誤差，不會制約影響此刻真實世界值和上一時刻估計值的誤差，即逗号前後二者獨立，協方差為0。
則參照前面的矩陣計算公式有：
設：
有：
矩陣具有這種性質：
依據以上性質，由于I是元素全為1的對稱陣， P(k│k-1)是方差陣，方差陣也是對稱陣，則可得：
由于卡爾曼濾波是一個不斷減小最優值和此刻真實世界值的誤差的方差的過程。P(k|k)是最優值和此刻真實世界值的誤差的方差矩陣，其對角線元素就是各個元素各自的誤差的方差。為了讓所有元素的方差最小，我們把每個元素的方差都加起來，即把P(k|k-1)的對角元素方差加起來，求取總的方內插補點，隻要方內插補點總和最小，則就實作了最優值和此刻真實世界值的誤差的方差最小化的目的。将方差矩陣對角元素相加的做法也稱為矩陣的迹，通常用tr表示。則:
現在，這個由方差矩陣對角元素總群組成的tr[P(k|k)]函數需要取最小值，他的大小收到Kg(k)這個權重矩陣的影響，我們必須找到最适合的Kg(k)使得和值最小。
數學上有導數為0取極值的方法，如下圖所示：
矩陣的迹也有求導的公式：
則我們用 tr[P(k|k)]對Kg(k)求導，得：
由于HP(k│k-1) HT和R(K)都是協方差矩陣，是以他們的轉置等于自身，則：
在了解第五個公式的過程中，不小心就推出了第三個公式了，即比例系數矩陣Kg(k)最适合的值已經得到。
那麼第五個公式就得以了解
将上式替換進P(k|k)表達式中:
至此，第五個公式已經了解