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