經(jīng)過看各種博客和文章，讓我最清楚明白的，是xiahouzuoxin 的博客，之后又看了一些國外的文獻(xiàn)進(jìn)行自己的理解-20161116
首先得明白P Q R這些矩陣的含義與來源
Q：過程激勵噪聲的協(xié)方差矩陣。翻譯成這個名字是由時間序列信號模型的觀點(diǎn)，平穩(wěn)隨機(jī)序列可以看成是由典型噪聲源激勵線性系統(tǒng)產(chǎn)生，故譯作過程激勵噪聲。
R：觀測噪聲的協(xié)方差矩陣
P：不斷迭代計(jì)算的估計(jì)誤差的協(xié)方差矩陣
kalman濾波的過程：
濾波器估計(jì)過程某一時刻的狀態(tài)，然后以（含噪聲的）測量變量的方式獲得反饋。因此卡爾曼濾波器可分為兩個部分：時間更新方程和測量更新方程。時間更新方程負(fù)責(zé)及時向前推算當(dāng)前狀態(tài)變量和誤差協(xié)方差估計(jì)的值，以便為下一個時間狀態(tài)構(gòu)造先驗(yàn)估計(jì)。測量更新方程負(fù)責(zé)反饋——也就是說，它將先驗(yàn)估計(jì)和新的測量變量結(jié)合以構(gòu)造改進(jìn)的后驗(yàn)估計(jì)。時間更新方程也可視為預(yù)估方程，測量更新方程可視為校正方程。最后的估計(jì)算法成為一種具有數(shù)值解的預(yù)估－校正算法。
典型的公式中，前兩個是時間更新方程，后三個是狀態(tài)更新方程（公式中（ ? 代表先驗(yàn)，?代表估計(jì)）
參數(shù)解釋和公式
這里有兩個空間，狀態(tài)空間n*1,觀測空間m*1，往往觀測量和狀態(tài)量為同一個量，比如我觀測機(jī)器人的位置(x,y)，同時我要估計(jì)的也是機(jī)器人的位置(x,y)，此時H觀測模型矩陣為單位矩陣。但是也存在不同的情況，比如我觀測的是機(jī)器人的速度和姿態(tài)，我估計(jì)的是機(jī)器人的位置，此時H觀測模型矩陣需要根據(jù)不同情況自己設(shè)定。
最后把讓我茅塞頓開的博文轉(zhuǎn)載過來
Kalman濾波器的歷史淵源
We are like dwarfs on the shoulders of giants, by whose grace we see farther than they. Our study of the works of the ancients enables us to give fresh life to their finer ideas, and rescue them from time’s oblivion and man’s neglect.
—— Peter of Blois, late twelfth century
太喜歡第一句話了，“我們是巨人肩膀上的矮人，巨人們的優(yōu)雅讓我么看得更比他們更遠(yuǎn)”，誰說不是呢？
說起Kalman濾波器的歷史，最早要追溯到17世紀(jì)，Roger Cotes開始研究最小均方問題。但由于缺少實(shí)際案例的支撐（那個時候哪來那么多雷達(dá)啊啥的這些信號?。珻otes的研究讓人看著顯得很模糊，因此在估計(jì)理論的發(fā)展中影響很小。17世紀(jì)中葉，最小均方估計(jì)（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用于月球運(yùn)動的估計(jì)，Leonard Euler在1749年、Pierre Laplace在1787分別用于木星和土星的運(yùn)動估計(jì)。Roger Boscovich在1755用最小均方估計(jì)地球的大小。1777年，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發(fā)明了最大似然估計(jì)算法。遞歸的最小均方估計(jì)理論是由Karl Gauss建立在1809年（好吧，他聲稱在1795年就完成了），當(dāng)時還有Adrien Legendre在1805年完成了這項(xiàng)工作，Robert Adrain在1808年完成的，至于到底誰是Boss，矮子們就別管了吧！
在1880年，丹麥的天文學(xué)家Thorvald Nicolai Thiele在之前最小均方估計(jì)的基礎(chǔ)上開發(fā)了一個遞歸算法，與Kalman濾波非常相似。在某些標(biāo)量的情況下，Thiele的濾波器與Kalman濾波器時等價的，Thiele提出了估計(jì)過程噪聲和測量噪聲中方差的方法（過程噪聲和測量噪聲是Kalman濾波器中關(guān)鍵的概念）。
上面提到的這么多研究估計(jì)理論的先驅(qū)，大多是天文學(xué)家而非數(shù)學(xué)家?，F(xiàn)在，大部分的理論貢獻(xiàn)都源自于實(shí)際的工程?！癟here is nothing so practical as a good theory”，應(yīng)該就是“實(shí)踐是檢驗(yàn)真理的唯一標(biāo)準(zhǔn)”之類吧。
現(xiàn)在，我們的控制論大Wiener終于出場了，還有那個叫Kolmogorov（柯爾莫戈洛夫）的神人。在19世紀(jì)40年代，Wiener設(shè)計(jì)了Wiener濾波器，然而，Wiener濾波器不是在狀態(tài)空間進(jìn)行的（這個學(xué)過Wiener濾波的就知道，它是直接從觀測空間z(n)=s(n)+w(n)進(jìn)行的濾波），Wiener是穩(wěn)態(tài)過程，它假設(shè)測量是通過過去無限多個值估計(jì)得到的。Wiener濾波器比Kalman濾波器具有更高的自然統(tǒng)計(jì)特性。這些也限制其只是更接近理想的模型，要直接用于實(shí)際工程中需要足夠的先驗(yàn)知識（要預(yù)知協(xié)方差矩陣），美國NASA曾花費(fèi)多年的時間研究維納理論，但依然沒有在空間導(dǎo)航中看到維納理論的實(shí)際應(yīng)用。
在1950末期，大部分工作開始對維納濾波器中協(xié)方差的先驗(yàn)知識通過狀態(tài)空間模型進(jìn)行描述。通過狀態(tài)空間表述后的算法就和今天看到的Kalman濾波已經(jīng)極其相似了。Johns Hopkins大學(xué)首先將這個算法用在了導(dǎo)彈跟蹤中，那時在RAND公司工作的Peter Swerling將它用在了衛(wèi)星軌道估計(jì)，Swerling實(shí)際上已經(jīng)推導(dǎo)出了（1959年發(fā)表的）無噪聲系統(tǒng)動力學(xué)的Kalman濾波器，在他的應(yīng)用中，他還考慮了使用非線性系統(tǒng)動力學(xué)和和測量方程?？梢赃@樣說，Swerling和發(fā)明Kalman濾波器是失之交臂，一線之隔。在kalman濾波器聞名于世之后，他還寫信到AIAA Journal聲討要獲得Kalman濾波器發(fā)明的榮譽(yù)（然而這時已經(jīng)給濾波器命名Kalman了）?？偨Y(jié)其失之交臂的原因，主要是Swerling沒有直接在論文中提出Kalman濾波器的理論，而只是在實(shí)踐中應(yīng)用。
Rudolph Kalman在1960年發(fā)現(xiàn)了離散時間系統(tǒng)的Kalman濾波器，這就是我們在今天各種教材上都能看到的，1961年Kalman和Bucy又推導(dǎo)了連續(xù)時間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計(jì)的角度推導(dǎo)出了Kalman濾波器方程。
目前，卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)。卡爾曼最初提出的形式現(xiàn)在一般稱為簡單卡爾曼濾波器。除此以外，還有施密特?cái)U(kuò)展濾波器、信息濾波器以及很多Bierman, Thornton開發(fā)的平方根濾波器的變種。也許最常見的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛存在。
從牛頓到卡爾曼
從現(xiàn)在開始，就要進(jìn)行Kalman濾波器探討之旅了，我們先回到高一，從物理中小車的勻加速直線運(yùn)動開始。
話說，有一輛質(zhì)量為m的小車，受恒定的力F，沿著r方向做勻加速直線運(yùn)動。已知小車在t-ΔT時刻的位移是s(t-1)，此時的速度為v(t-1)。求：t時刻的位移是s(t)，速度為v(t)？
由牛頓第二定律，求得加速度：
那么就有下面的位移和速度關(guān)系：
如果將上面的表達(dá)式用矩陣寫在一起，就變成下面這樣：
卡爾曼濾波器是建立在動態(tài)過程之上，由于物理量（位移，速度）的不可突變特性，這樣就可以通過t-1時刻估計(jì)（預(yù)測）t時刻的狀態(tài)，其狀態(tài)空間模型為：
對比一下(1)(2)式，長得及其相似有木有：
勻加速直線運(yùn)動過程就是卡爾曼濾波中狀態(tài)空間模型的一個典型應(yīng)用。下面我們重點(diǎn)關(guān)注(2)式，鑒于研究的計(jì)算機(jī)信號都是離散的，將(2)是表示成離散形式為：
其中各個量之間的含義是：
x(n)是狀態(tài)向量，包含了觀測的目標(biāo)（如：位移、速度）
u(n)是驅(qū)動輸入向量，如上面的運(yùn)動過程是通過受力驅(qū)動產(chǎn)生加速度，所以u(n)和受力有關(guān)
A是狀態(tài)轉(zhuǎn)移矩陣，其隱含指示了“n-1時刻的狀態(tài)會影響到n時刻的狀態(tài)（這似乎和馬爾可夫過程有些類似）”
B是控制輸入矩陣，其隱含指示了“n時刻給的驅(qū)動如何影響n時刻的狀態(tài)”
從運(yùn)動的角度，很容易理解：小車當(dāng)前n時刻的位移和速度一部分來自于n-1時刻的慣性作用，這通過Ax(n)來度量，另一部分來自于現(xiàn)在n時刻小車新增加的外部受力，通過Bu(n)來度量。
w(n)是過程噪聲，w(n)~N(0,Q)的高斯分布，過程噪聲是使用卡爾曼濾波器時一個重要的量，后面會進(jìn)行分析。
計(jì)算n時刻的位移，還有一種方法：拿一把長的卷尺（嗯，如果小車跑了很長時間，估計(jì)這把卷尺就難買到了），從起點(diǎn)一拉，直接就出來了，設(shè)測量值為z(n)。計(jì)算速度呢？速度傳感器往那一用就出來了。
然而，初中物理就告訴我們，“尺子是量不準(zhǔn)的，物體的物理真實(shí)值無法獲得”，測量存在誤差，我們暫且將這個誤差記為v(n)。這種通過直接測量的方式獲得所需物理量的值構(gòu)成觀測空間：
z(n)就是測量結(jié)果，H(n)是觀測矢量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)為測量噪聲，同狀態(tài)空間方程中的過程噪聲一樣，這也是一個后面要討論的量。大部分情況下，如果物理量能直接通過傳感器測量，
現(xiàn)在就有了兩種方法（如上圖）可以得到n時刻的位移和速度：一種就是通過(3)式的狀態(tài)空間遞推計(jì)算（Prediction），另一種就是通過(4)式直接拿尺子和傳感器測量（Measurement）。致命的是沒一個是精確無誤的，就像上圖看到的一樣，分別都存在0均值高斯分布的誤差w(n)和v(n)。
那么，我最終的結(jié)果是取尺子量出來的好呢，還是根據(jù)我們偉大的牛頓第二定律推導(dǎo)出來的好呢？抑或兩者都不是！
一場遞推的游戲
為充分利用測量值（Measurement）和預(yù)測值（Prediction），Kalman濾波并不是簡單的取其中一個作為輸出，也不是求平均。
設(shè)預(yù)測過程噪聲w(n)N(0,Q)，測量噪聲v(n)N(0,R)。Kalman計(jì)算輸出分為預(yù)測過程和修正過程如下：
預(yù)測
預(yù)測值：
最小均方誤差矩陣：
修正
誤差增益：
修正值：
最小均方誤差矩陣：
從(5)~(9)中：
x(n)：Nx1的狀態(tài)矢量
z(n)：Mx1的觀測矢量，Kalman濾波器的輸入
x(n|n-1)：用n時刻以前的數(shù)據(jù)進(jìn)行對n時刻的估計(jì)結(jié)果
x(n|n)：用n時刻及n時刻以前的數(shù)據(jù)對n時刻的估計(jì)結(jié)果，這也是Kalman濾波器的輸出
P(n|n-1)：NxN，最小預(yù)測均方誤差矩陣，其定義式為
通過計(jì)算最終得到(6)式。
P(n|n)：NxN，修正后最小均方誤差矩陣。
K(n)：NxM，誤差增益，從增益的表達(dá)式看，相當(dāng)于“預(yù)測最小均方誤差”除以“n時刻的測量誤差+預(yù)測最小均方誤差”，直觀含義就是用n-1預(yù)測n時刻狀態(tài)的預(yù)測最小均方誤差在n時刻的總誤差中的比重，比重越大，說明真值接近預(yù)測值的概率越?。ń咏鼫y量值的概率越大），這也可以從(8)式中看到。
Kalman濾波算法的步驟是(5)(6)->(7)->(8)(9)。當(dāng)然，建議找本教材復(fù)習(xí)下上面公式的推導(dǎo)過程，或參見Wiki上的介紹http://en.wikipedia.org/wiki/Kalman_filter。
公式就是那么的抽象，一旦認(rèn)真研究懂了卻也是茅塞頓開，受益也比只知皮毛的多。盡管如此，我還算更喜歡先感性后理性。仍以上面的運(yùn)動的例子來直觀分析：
Example:
還可以更簡單一些：設(shè)小車做勻速（而非勻加速）直線運(yùn)動，方便計(jì)算，假設(shè)速度絕對的恒定（不波動，所以相關(guān)的方差都為0），則u(t)==0恒成立。設(shè)預(yù)測（過程）位移噪聲w(n)N(0,2^2)，測量位移噪聲v(n)N(0,1^2)，n-1狀態(tài)的位移
則A = [1 ΔT; 0 1]=[1 1; 0 1]，根據(jù)(5)，預(yù)測值為
現(xiàn)在已經(jīng)有了估計(jì)值和測量值，哪個更接近真值，這就通過最小均方誤差矩陣來決定！
要求已知上次的修正后的最小均方誤差P(n-1|n-1)=[1 0; 0 0]（勻速，所以P(2,2)=0，右斜對角線上為協(xié)方差，值為0，P(1,1)為n-1時刻位移量的均方誤差，因?yàn)橐?jì)算P(1,1)還得先遞推往前計(jì)算P(n-2|n-2)，所以這里暫時假設(shè)為1），則根據(jù)(6)式，最小預(yù)測預(yù)測均方誤差為P(n|n-1)=[1 0; 0 0][1 1; 0 1][1 0; 0 0]=[1 0; 0 0]。
由物理量的關(guān)系知，H(n)=[1 1]，增益K(n)=[1;0]{1+[1 1][1 0; 0 0][1; 1]}^(-1)=[1/2;0]。
所以，最后的n時刻估計(jì)值既不是用n-1得到的估計(jì)值，也不是測量值，而是：
從上面的遞推關(guān)系知道，要估計(jì)n時刻就必須知道n-1時刻，那么n=0時刻該如何估計(jì)，因此，卡爾曼濾波要初始化的估計(jì)值x(-1|-1)和誤差矩陣P(-1|-1)，設(shè)x(-1,-1)~N(Us, Cs)，則初始化：
綜上，借用一張圖說明一下Kalman濾波算法的流程：
圖中的符號和本文符號稍有差異，主要是P的表示上。從上圖也可以看出，Kalman濾波就是給定-1時刻的初始值，然后在預(yù)測（狀態(tài)空間）和修正（觀測空間）之間不停的遞推，求取n時刻的估計(jì)x和均方誤差矩陣P。
均方誤差中的門道
到這里，應(yīng)該對Kalman濾波有個總體的概念了，有幾個觀點(diǎn)很重要，是建立Kalman濾波器的基礎(chǔ)：
一個是n-1對n時刻估計(jì)值，一個是n時刻的測量值，估計(jì)值和測量值都存在誤差，且誤差都假設(shè)滿足獨(dú)立的高斯分布
Kalman濾波器就是充分結(jié)合了估計(jì)值和測量值得到n時刻更接近真值的估計(jì)結(jié)果
Kalman濾波器引入狀態(tài)空間的目的是避免了“像Wiener濾波器一樣需要對過去所有[0,n-1]時刻協(xié)方差先驗(yàn)知識都已知”，而直接可以通過上一時刻即n-1時刻的狀態(tài)信息和均方誤差信息就可遞推得到n時刻的估計(jì)。盡管遞推使得實(shí)際應(yīng)用中方便了，但n-1對n時刻的估計(jì)實(shí)際上使用到了所有前[0,n-1]時刻的信息，只不過信息一直通過最小均方誤差進(jìn)行傳遞到n-1時刻?；诖耍琄alman濾波也需要先驗(yàn)知識，即-1時刻的初始值。
在上小節(jié)中只看到Kalman的結(jié)論，那么Kalman濾波器是如何將估計(jì)值和測量值結(jié)合起來，如何將信息傳遞下去的呢？這其中，“獨(dú)立高斯分布”的假設(shè)條件功勞不可謂不大！測量值z(n)N(uz,σz^2)，估計(jì)值x(n)N(ux,σx^2)。
Kalman濾波器巧妙的用“獨(dú)立高斯分布的乘積”將這兩個測量值和估計(jì)值進(jìn)行融合！
如下圖：估計(jì)量的高斯分布和測量量的高斯分布經(jīng)過融合后為綠色的高斯分布曲線。
稍微計(jì)算一下，通過上式求出u和σ^2，
現(xiàn)在令
則(10)(11)變成：
到這里，請將(13)-(14)與(8)-(9)式對比！標(biāo)量的情況下，在小車的應(yīng)用中有：A=1，H=1，正態(tài)分布的均值u就是我們要的輸出結(jié)果，正態(tài)分布的方差σz^2就是最小均方誤差。推廣到矢量的情況，最小均方誤差矩陣就是多維正態(tài)分布的協(xié)方差矩陣。
從(12)式也很容易看到卡爾曼增益K的含義：就是估計(jì)量的方差占總方差（包括估計(jì)方差和測量方差）的比重。
一切都變得晴朗起來了，然而這一切的一切，卻都源自于“估計(jì)量和測量量的獨(dú)立高斯分布”這條假設(shè)。進(jìn)一步總結(jié)Kalman濾波器：
假設(shè)狀態(tài)空間的n-1時刻估計(jì)值和觀測空間的n時刻測量值都滿足獨(dú)立高斯分布，Kalman濾波器就是通過高斯分布的乘積運(yùn)算將估計(jì)值和測量值結(jié)合，獲得最接近真值的n時刻估計(jì)。
高斯分布乘積運(yùn)算的結(jié)果仍為高斯分布，高斯分布的均值對應(yīng)n時刻的估計(jì)值，高斯分布的方差對應(yīng)n時刻的均方誤差。
Matlab程序看過來
下面的一段Matlab代碼是從網(wǎng)上找到的，程序簡單直接，但作為學(xué)習(xí)分析用很棒
該程序中使用的符號的含義與本文一致，函數(shù)前的注釋再清晰不過了，就不多說。下面是一段[測試]
Kalman的參數(shù)中s.Q和s.R的設(shè)置非常重要，之前也提過，一般要通過實(shí)驗(yàn)統(tǒng)計(jì)得到，它們分布代表了狀態(tài)空間估計(jì)的誤差和測量的誤差。
Kalman濾波器的效果是使輸出變得更平滑，但沒辦法去除信號中原有的椒鹽噪聲，而且，Kalman濾波器也會跟蹤這些椒鹽噪聲點(diǎn)，因此推薦在使用Kalman濾波器前先使用中值濾波去除椒鹽噪聲。
Kalman濾波C程序
我就在上面公式的基礎(chǔ)上實(shí)現(xiàn)了基本的Kalman濾波器，包括1維和2維狀態(tài)的情況。先在頭文件中聲明1維和2維Kalman濾波器結(jié)構(gòu)：
我都給了有詳細(xì)的注釋，kalman1_state
是狀態(tài)空間為1維/測量空間1維的Kalman濾波器，kalman2_state
是狀態(tài)空間為2維/測量空間1維的Kalman濾波器。兩個結(jié)構(gòu)體都需要通過初始化函數(shù)初始化相關(guān)參數(shù)、狀態(tài)值和均方差值。
其實(shí)，Kalman濾波器由于其遞推特性，實(shí)現(xiàn)起來很簡單。但調(diào)參有很多可研究的地方，主要需要設(shè)定的參數(shù)如下：
init_x：待測量的初始值，如有中值一般設(shè)成中值（如陀螺儀）
init_p：后驗(yàn)狀態(tài)估計(jì)值誤差的方差的初始值
q：預(yù)測（過程）噪聲方差
r：測量（觀測）噪聲方差。以陀螺儀為例，測試方法是：保持陀螺儀不動，統(tǒng)計(jì)一段時間內(nèi)的陀螺儀輸出數(shù)據(jù)。數(shù)據(jù)會近似正態(tài)分布，按3σ原則，取正態(tài)分布的(3σ)^2作為r的初始化值。
其中q和r參數(shù)尤為重要，一般得通過實(shí)驗(yàn)測試得到。
找兩組聲陣列測向的角度數(shù)據(jù)，對上面的C程序進(jìn)行測試。一維Kalman（一維也是標(biāo)量的情況，就我所知，現(xiàn)在網(wǎng)上看到的代碼大都是使用標(biāo)量的情況）和二維Kalman（一個狀態(tài)是角度值，另一個狀態(tài)是向量角度差，也就是角速度）的結(jié)果都在圖中顯示。這里再稍微提醒一下：狀態(tài)量不要取那些能突變的量，如加速度，這點(diǎn)在文章“從牛頓到卡爾曼”一小節(jié)就提到過。
Matlab繪出的跟蹤結(jié)果顯示：
Kalman濾波結(jié)果比原信號更平滑。但是有椒鹽突變噪聲的地方，Kalman濾波器并不能濾除椒鹽噪聲的影響，也會跟蹤椒鹽噪聲點(diǎn)。因此，推薦在Kalman濾波器之前先使用中值濾波算法去除椒鹽突變點(diǎn)的影響。
上面所有C程序的源代碼及測試程序都公布在我的Github上，希望大家批評指正其中可能存在的錯誤。
