前言：
從上個世紀卡爾曼濾波理論被提出，卡爾曼濾波在控制論與信息論的連接上做出了卓越的貢獻。為了得出準確的下一時刻狀態(tài)真值，我們常常使用卡爾曼濾波、擴展卡爾曼濾波、無跡卡爾曼濾波、粒子濾波等等方法，這些方法在姿態(tài)解算、軌跡規(guī)劃等方面有著很多用途。卡爾曼濾波的本質(zhì)是參數(shù)化的貝葉斯模型，通過對下一時刻系統(tǒng)的初步狀態(tài)估計（即狀態(tài)的先驗估計）以及測量得出的反饋相結(jié)合，最終得到改時刻較為準確的的狀態(tài)估計（即狀態(tài)的后驗估計），其核心思想即為預測+測量反饋，而這兩者是通過一個變化的權(quán)值相聯(lián)系使得最后的狀態(tài)后驗估計無限逼近系統(tǒng)準確的狀態(tài)真值，這個權(quán)值即為大名鼎鼎的卡爾曼增益。可以說，卡爾曼濾波并不與傳統(tǒng)的在頻域的濾波相似，而是一種在時域的狀態(tài)預測器，這就省去了時域頻域的變換的步驟，而這種狀態(tài)預測器不僅僅在工程上有很廣的應用，在金融方面例如股票的走勢等等方面也可以有很多的應用。
一直以來，卡爾曼濾波相關(guān)的資料較少，很多人覺得很難，因此寫下這篇博客來幫助大家更好的理解卡爾曼濾波的運作原理。
2017年2月11日 by 回憶不能已 轉(zhuǎn)載請聲明出處 謝謝合作
————————————————————————————————————————————————————————————————————————————————————
一、卡爾曼濾波的定義
卡爾曼濾波的定義：
一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。
1.1 線性系統(tǒng)狀態(tài)方程
首先我們來看什么是線性系統(tǒng)狀態(tài)方程，這個名詞包含線性系統(tǒng)、狀態(tài)這兩個特征。
線性系統(tǒng)
狀態(tài)空間描述（內(nèi)部描述）：基于系統(tǒng)內(nèi)部結(jié)構(gòu)，是對系統(tǒng)的一種完整的描述。
狀態(tài)方程
描述系統(tǒng)狀態(tài)變量間或狀態(tài)變量與系統(tǒng)輸入變量間關(guān)系的一個一階微分方程組(連續(xù)系統(tǒng))或一階差分方程組（離散系統(tǒng)），稱為狀態(tài)方程。
1.2 觀測數(shù)據(jù)
觀測數(shù)據(jù)代表傳感器采集的實際數(shù)據(jù)，可能存在著或多或少的誤差，例如陀螺儀的積分誤差等。
1.3 最優(yōu)估計
最優(yōu)估計指的是使經(jīng)過KF算法解算的數(shù)據(jù)無限接近于真實值的估計，用數(shù)學表述即為后驗概率估計無限接近于真實值。
二、卡爾曼濾波算法流程
卡爾曼濾波算法核心思想在于預測+測量反饋，它由兩部分組成，第一部分是 線性系統(tǒng)狀態(tài)預測方程，第二部分是 線性系統(tǒng)觀測方程。
2.1 線性系統(tǒng)狀態(tài)預測方程
A ：表示狀態(tài)轉(zhuǎn)移系數(shù)矩陣，n×n 階
B ：表示可選的控制輸入的增益矩陣
Q ：表示過程激勵噪聲的協(xié)方差矩陣
上圖為線性系統(tǒng)狀態(tài)預測方程的表達式，假設(shè)過程激勵噪聲滿足高斯分布。
2.2 線性系統(tǒng)觀測方程
H ：表示量測系數(shù)矩陣，m×n 階矩陣
R ：表示測量噪聲協(xié)方差矩陣
上圖為線性系統(tǒng)觀測方程表達式，假設(shè)測量噪聲矩陣滿足高斯分布
隨機信號 wk和vk分別表示過程激勵噪聲和觀測噪聲。Kf算法中假設(shè)它們?yōu)橄嗷オ毩ⅲ龖B(tài)分布的白色噪聲。
預測+測量反饋進行后續(xù)的估計，那kf算法到底是如何利用預測狀態(tài)與測量的反饋進行最優(yōu)估計的呢？這部分我們將在后續(xù)的模型及數(shù)學推導來詳細講解
2.3 擴展卡爾曼濾波EKF流程
EKF的基本思想是將非線性系統(tǒng)線性化，然后進行卡爾曼濾波，因此EKF是一種偽非線性的卡爾曼濾波。實際中一階EKF應用廣泛。
但EKF存在一定的局限性：
其一是當強非線性時EKF違背局部線性假設(shè)，Taylor展開式中被忽略的高階項帶來大的誤差時，EKF算法可能會使濾波發(fā)散；
另外，由于EKF在線性化處理時需要用雅克比(Jacobian)矩陣，其繁瑣的計算過程導致該方法實現(xiàn)相對困難。所以，在滿足線性系統(tǒng)、高斯白噪聲、所有隨機變量服從高斯(Gaussian)分布這3個假設(shè)條件時，EKF是最小方差準則下的次優(yōu)濾波器，其性能依賴于局部非線性度。
以PIXHAWK飛控代碼為例說明：
1、非線性系統(tǒng)線性化
EKF對非線性函數(shù)的Taylor展開式進行一階線性化截斷，忽略其余高階項，從而將非線性問題轉(zhuǎn)化為線性，可以將卡爾曼線性濾波算法應用于非線性系統(tǒng)中多種二階廣義卡爾曼濾波方法的提出及應用進一步提高了卡爾曼濾波對非線性系統(tǒng)的估計性能
2、二階濾波方法考慮了Taylor級數(shù)展開的二次項，因此減少了由于線性化所引起的估計誤差，但大大增加了運算量
3、pixhawk開源項目使用的是一階EKF濾波，因為二到三階濾波計算量較大如果飛行器的處理速度能跟上，可以考慮2-3階濾波。
擴展卡爾曼濾波的狀態(tài)預測方程以及觀測方程與一般的卡爾曼濾波略有不同：
KF與EKF的聯(lián)系如下圖所示：
三、卡爾曼濾波算法模型
3.1 卡爾曼濾波算法應用前提
(3)假設(shè)系統(tǒng)的噪聲統(tǒng)計特性可觀測
應用中大多數(shù)情況下并不知道建模噪聲和觀測噪聲的統(tǒng)計特性，一般假定為零均值，方差的值通過測試或是經(jīng)驗豐富的開發(fā)人員設(shè)置得到，噪聲一般直接簡化假設(shè)為高斯白噪聲
3.2 預先的一些數(shù)學知識準備
先驗估計：先驗狀態(tài)估計是根據(jù)系統(tǒng)過程原理或者經(jīng)驗得到的估計值，實際應用中可以通過傳感器數(shù)據(jù)去預測下一時刻的數(shù)據(jù)
后驗估計：后驗狀態(tài)估計是結(jié)合之前的先驗狀態(tài)估計值，再加權(quán)測量值得到一個理論上最接近真實值的結(jié)果
協(xié)方差：協(xié)方差用于衡量兩個變量的總體誤差。而方差是協(xié)方差的一種特殊情況，即當兩個變量是相同的情況
均方差：它是“誤差”的平方的期望值
矩陣的跡：矩陣主對角線上的所有元素之和稱之為矩陣的跡
協(xié)方差矩陣的主對角線元素分別為兩個變量的方差
最小均方差估計就是指估計參數(shù)時要使得估計出來的模型和真實值之間的誤差平方期望值最小。
3.3 模型講解
實際的狀態(tài)模型如下所示：
隨機信號w和v分別表示過程激勵噪聲和觀測噪聲。
假設(shè)它們?yōu)橄嗷オ毩ⅲ龖B(tài)分布的白色噪聲
圖片中藍色標記的為已知的矩陣和信息，卡爾曼濾波的目的就是利用這些已知的信息估計離散時間過程的狀態(tài)變量。為什么是估計呢？因為有噪聲、 存在。而信息就這么多，我們要做的就是利用這手頭的信息盡量準確的估計出離散過程的狀態(tài)變量，從而隨時掌握系統(tǒng)的運行狀態(tài)和變化。
舉個日常生活的例子：
假設(shè)你有兩個傳感器，測的是同一個信號。可是它們每次的讀數(shù)都不太一樣，怎么辦？取平均。
再假設(shè)你知道其中貴的那個傳感器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？加權(quán)平均。
怎么加權(quán)？假設(shè)兩個傳感器的誤差都符合正態(tài)分布，假設(shè)你知道這兩個正態(tài)分布的方差，用這兩個方差值，（此處省略若干數(shù)學公式），你可以得到一個“最優(yōu)”的權(quán)重。
接下來，重點來了：
假設(shè)你只有一個傳感器，但是你還有一個數(shù)學模型。模型可以幫你算出一個值，但也不是那么準。怎么辦？把模型算出來的值，和傳感器測出的值，（就像兩個傳感器那樣），取加權(quán)平均。OK，最后一點說明：你的模型其實只是一個步長的，也就是說，知道x(k)，我可以求x(k+1)。問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權(quán)平均之后的那個、對x在k時刻的最佳估計值。于是迭代也有了。
卡爾曼濾波的模型：
帶卡爾曼濾波器的系統(tǒng)方框圖，圖中的上半部分是實際的離散時間過程（有噪聲的存在），下面部分是卡爾曼濾波器。卡爾曼濾波器通過利用藍色標記的可用信息對系統(tǒng)的狀態(tài)變量進行估計，得出粉色標記的方框，而這個得以實現(xiàn)的關(guān)鍵就是紅色標記出的反饋信息的利用，即設(shè)置怎樣的反饋增益矩陣K使狀態(tài)變量在某種意義下最準確，最接近真實系統(tǒng)狀態(tài)。
我們認定是預測（先驗）值，是估計值，為測量值的預測，在下面的推導中，請注意估計和預測兩者的區(qū)別，不混為一談。由一般的反饋思想我們得到估計值：
其中括號里面的公式稱之為殘差，也就是預測的和你實際測量值之間的差距。如果這項等于0，說明預測和測量出的完全吻合。
關(guān)鍵就是求取這個K。這時最小均方差就起到了作用。
順便在這里回答為什么噪聲必須服從高斯分布，在進行參數(shù)估計的時候，估計的一種標準叫最大似然估計，它的核心思想就是你手里的這些相互間獨立的樣本既然出現(xiàn)了，那就說明這些樣本概率的乘積應該最大（概率大才出現(xiàn)嘛）。如果樣本服從概率高斯分布，對他們的概率乘積取對數(shù)ln后，你會發(fā)現(xiàn)函數(shù)形式將會變成一個常數(shù)加上樣本最小均方差的形式。因此，看似直觀上很容易理解的最小均方差理論上來源就出于那里。
我們想得到最優(yōu)的估計，那么必須知道k時刻的后驗估計協(xié)方差與k時刻的先驗估計協(xié)方差。
k時刻的后驗估計協(xié)方差表達式為
紅色方框里面表達式在前面提過，即
k時刻的先驗估計協(xié)方差表達式為
我們把k時刻的后驗估計協(xié)方差矩陣的跡即為T[Pk]
Pk對角線元素的和即為均方差。
x^kˉ：表示k時刻先驗狀態(tài)估計值，這是算法根據(jù)前次迭代結(jié)果（就是上一次循環(huán)的后驗估計值）做出的不可靠估計。x^k、x^k?1：分別表示k時刻、k-1時刻后驗狀態(tài)估計值，也就是要輸出的該時刻最優(yōu)估計值，這個值是卡爾曼濾波的結(jié)果。 A ：表示狀態(tài)轉(zhuǎn)移矩陣，是n×n階方陣，它是算法對狀態(tài)變量進行預測的依據(jù)，狀態(tài)轉(zhuǎn)移矩陣如果不符合目標模型有可能導致濾波發(fā)散，它的確定請參看第二節(jié)中的舉例。
B ：表示可選的控制輸入u∈Rl的增益，在大多數(shù)實際情況下并沒有控制增益。
uk?1：表示k-1時刻的控制增益，一般沒有這個變量，可以設(shè)為0。
P^kˉ ：表示k時刻的先驗估計協(xié)方差，這個協(xié)方差矩陣只要確定了一開始的P^0，后面都可以遞推出來，而且初始協(xié)方差矩陣P^0只要不是為0，它的取值對濾波效果影響很小，都能很快收斂。
P^k 、P^k?1 ：分別表示k時刻、k-1時刻的后驗估計協(xié)方差，是濾波結(jié)果之一。
Q：表示過程激勵噪聲的協(xié)方差，它是狀態(tài)轉(zhuǎn)移矩陣與實際過程之間的誤差。這個矩陣是卡爾曼濾波中比較難確定的一個量，一般有兩種思路：一是在某些穩(wěn)定的過程可以假定它是固定的矩陣，通過尋找最優(yōu)的Q值使濾波器獲得更好的性能，這是調(diào)整濾波器參數(shù)的主要手段，Q一般是對角陣，且對角線上的值很小，便于快速收斂；二是在自適應卡爾曼濾波（AKF）中Q矩陣是隨時間變化的。
Kk ：表示卡爾曼增益，是濾波的中間結(jié)果。
zk ：表示測量值，是m階向量。
H：表示量測矩陣，是m×n階矩陣，它把m維測量值轉(zhuǎn)換到n維與狀態(tài)變量相對應。
R：表示測量噪聲協(xié)方差，它是一個數(shù)值，這是和儀器相關(guān)的一個特性，作為已知條件輸入濾波器。需要注意的是這個值過大過小都會使濾波效果變差，且R取值越小收斂越快，所以可以通過實驗手段尋找合適的R值再利用它進行真實的濾波。
最小均方差求解
均方差對未知量K求導,令導函數(shù)等于0，就能確定卡爾曼增益K的值，完成模型的最小均方差估計，從而使后驗估計的誤差很小，更加逼近狀態(tài)的真值
求最小均方差確定卡爾曼增益的表達式
H：表示量測矩陣
R：表示測量噪聲協(xié)方差，和儀器相關(guān)
3.4 卡爾曼增益的分析
先驗估計誤差較大，我們需要更加信任測量反饋，模型自動加大卡爾曼增益來進行更準確的估計
先驗估計沒有任何誤差，我們只需要通過預測就能夠得到準確的后驗估計，無需測量的反饋，模型自動將測量反饋的權(quán)值設(shè)為0
3.5模型的迭代過程
3.5.1時間更新方程
A ：表示狀態(tài)轉(zhuǎn)移矩陣，n×n 階
B ：表示可選的控制輸入的增益矩陣
Q ：表示過程激勵噪聲的協(xié)方差矩陣
3.5.2 狀態(tài)更新方程
H ：表示量測矩陣，m×n 階矩陣
R ：表示測量噪聲協(xié)方差矩陣
3.5.3 迭代示意圖
下篇博客會詳細分析EKF的模型，對其非線性系統(tǒng)的建模進行講解，歡迎大家指出博客中的錯誤！
