經(jīng)過(guò)看各種博客和文章遵馆，讓我最清楚明白的，是xiahouzuoxin 的博客骨饿，之后又看了一些國(guó)外的文獻(xiàn)進(jìn)行自己的理解-20161116
首先得明白P Q R這些矩陣的含義與來(lái)源
Q：過(guò)程激勵(lì)噪聲的協(xié)方差矩陣宏赘。翻譯成這個(gè)名字是由時(shí)間序列信號(hào)模型的觀點(diǎn)绒北，平穩(wěn)隨機(jī)序列可以看成是由典型噪聲源激勵(lì)線性系統(tǒng)產(chǎn)生，故譯作過(guò)程激勵(lì)噪聲察署。
R：觀測(cè)噪聲的協(xié)方差矩陣
P：不斷迭代計(jì)算的估計(jì)誤差的協(xié)方差矩陣
kalman濾波的過(guò)程：
濾波器估計(jì)過(guò)程某一時(shí)刻的狀態(tài)闷游，然后以（含噪聲的）測(cè)量變量的方式獲得反饋。因此卡爾曼濾波器可分為兩個(gè)部分：時(shí)間更新方程和測(cè)量更新方程。時(shí)間更新方程負(fù)責(zé)及時(shí)向前推算當(dāng)前狀態(tài)變量和誤差協(xié)方差估計(jì)的值脐往，以便為下一個(gè)時(shí)間狀態(tài)構(gòu)造先驗(yàn)估計(jì)休吠。測(cè)量更新方程負(fù)責(zé)反饋——也就是說(shuō)，它將先驗(yàn)估計(jì)和新的測(cè)量變量結(jié)合以構(gòu)造改進(jìn)的后驗(yàn)估計(jì)业簿。時(shí)間更新方程也可視為預(yù)估方程瘤礁，測(cè)量更新方程可視為校正方程。最后的估計(jì)算法成為一種具有數(shù)值解的預(yù)估－校正算法梅尤。
典型的公式中柜思，前兩個(gè)是時(shí)間更新方程，后三個(gè)是狀態(tài)更新方程（公式中（ ? 代表先驗(yàn)巷燥，?代表估計(jì)）
參數(shù)解釋和公式
這里有兩個(gè)空間赡盘，狀態(tài)空間n*1,觀測(cè)空間m*1，往往觀測(cè)量和狀態(tài)量為同一個(gè)量矾湃，比如我觀測(cè)機(jī)器人的位置(x,y)亡脑，同時(shí)我要估計(jì)的也是機(jī)器人的位置(x,y)，此時(shí)H觀測(cè)模型矩陣為單位矩陣邀跃。但是也存在不同的情況霉咨，比如我觀測(cè)的是機(jī)器人的速度和姿態(tài)，我估計(jì)的是機(jī)器人的位置拍屑，此時(shí)H觀測(cè)模型矩陣需要根據(jù)不同情況自己設(shè)定途戒。
最后把讓我茅塞頓開(kāi)的博文轉(zhuǎn)載過(guò)來(lái)
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)”喷斋，誰(shuí)說(shuō)不是呢？
說(shuō)起Kalman濾波器的歷史蒜茴，最早要追溯到17世紀(jì)星爪，Roger Cotes開(kāi)始研究最小均方問(wèn)題。但由于缺少實(shí)際案例的支撐（那個(gè)時(shí)候哪來(lái)那么多雷達(dá)啊啥的這些信號(hào)胺鬯健）顽腾，Cotes的研究讓人看著顯得很模糊，因此在估計(jì)理論的發(fā)展中影響很小诺核。17世紀(jì)中葉抄肖，最小均方估計(jì)（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用于月球運(yùn)動(dòng)的估計(jì)窖杀，Leonard Euler在1749年漓摩、Pierre Laplace在1787分別用于木星和土星的運(yùn)動(dòng)估計(jì)。Roger Boscovich在1755用最小均方估計(jì)地球的大小入客。1777年管毙，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發(fā)明了最大似然估計(jì)算法腿椎。遞歸的最小均方估計(jì)理論是由Karl Gauss建立在1809年（好吧，他聲稱(chēng)在1795年就完成了）锅风，當(dāng)時(shí)還有Adrien Legendre在1805年完成了這項(xiàng)工作酥诽，Robert Adrain在1808年完成的，至于到底誰(shuí)是Boss皱埠，矮子們就別管了吧肮帐！
在1880年，丹麥的天文學(xué)家Thorvald Nicolai Thiele在之前最小均方估計(jì)的基礎(chǔ)上開(kāi)發(fā)了一個(gè)遞歸算法边器，與Kalman濾波非常相似训枢。在某些標(biāo)量的情況下，Thiele的濾波器與Kalman濾波器時(shí)等價(jià)的忘巧，Thiele提出了估計(jì)過(guò)程噪聲和測(cè)量噪聲中方差的方法（過(guò)程噪聲和測(cè)量噪聲是Kalman濾波器中關(guān)鍵的概念）恒界。
上面提到的這么多研究估計(jì)理論的先驅(qū)，大多是天文學(xué)家而非數(shù)學(xué)家⊙庾欤現(xiàn)在十酣，大部分的理論貢獻(xiàn)都源自于實(shí)際的工程〖食ぃ“There is nothing so practical as a good theory”耸采，應(yīng)該就是“實(shí)踐是檢驗(yàn)真理的唯一標(biāo)準(zhǔn)”之類(lèi)吧。
現(xiàn)在工育，我們的控制論大Wiener終于出場(chǎng)了虾宇，還有那個(gè)叫Kolmogorov（柯?tīng)柲曷宸颍┑纳袢恕Ｔ?9世紀(jì)40年代如绸，Wiener設(shè)計(jì)了Wiener濾波器嘱朽，然而，Wiener濾波器不是在狀態(tài)空間進(jìn)行的（這個(gè)學(xué)過(guò)Wiener濾波的就知道怔接，它是直接從觀測(cè)空間z(n)=s(n)+w(n)進(jìn)行的濾波）搪泳，Wiener是穩(wěn)態(tài)過(guò)程，它假設(shè)測(cè)量是通過(guò)過(guò)去無(wú)限多個(gè)值估計(jì)得到的扼脐。Wiener濾波器比Kalman濾波器具有更高的自然統(tǒng)計(jì)特性岸军。這些也限制其只是更接近理想的模型，要直接用于實(shí)際工程中需要足夠的先驗(yàn)知識(shí)（要預(yù)知協(xié)方差矩陣）谎势，美國(guó)NASA曾花費(fèi)多年的時(shí)間研究維納理論，但依然沒(méi)有在空間導(dǎo)航中看到維納理論的實(shí)際應(yīng)用杨名。
在1950末期脏榆，大部分工作開(kāi)始對(duì)維納濾波器中協(xié)方差的先驗(yàn)知識(shí)通過(guò)狀態(tài)空間模型進(jìn)行描述。通過(guò)狀態(tài)空間表述后的算法就和今天看到的Kalman濾波已經(jīng)極其相似了台谍。Johns Hopkins大學(xué)首先將這個(gè)算法用在了導(dǎo)彈跟蹤中须喂，那時(shí)在RAND公司工作的Peter Swerling將它用在了衛(wèi)星軌道估計(jì)，Swerling實(shí)際上已經(jīng)推導(dǎo)出了（1959年發(fā)表的）無(wú)噪聲系統(tǒng)動(dòng)力學(xué)的Kalman濾波器，在他的應(yīng)用中坞生，他還考慮了使用非線性系統(tǒng)動(dòng)力學(xué)和和測(cè)量方程仔役。可以這樣說(shuō)是己，Swerling和發(fā)明Kalman濾波器是失之交臂又兵，一線之隔。在kalman濾波器聞名于世之后卒废，他還寫(xiě)信到AIAA Journal聲討要獲得Kalman濾波器發(fā)明的榮譽(yù)（然而這時(shí)已經(jīng)給濾波器命名Kalman了）沛厨。總結(jié)其失之交臂的原因摔认，主要是Swerling沒(méi)有直接在論文中提出Kalman濾波器的理論逆皮，而只是在實(shí)踐中應(yīng)用。
Rudolph Kalman在1960年發(fā)現(xiàn)了離散時(shí)間系統(tǒng)的Kalman濾波器参袱，這就是我們?cè)诮裉旄鞣N教材上都能看到的电谣，1961年Kalman和Bucy又推導(dǎo)了連續(xù)時(shí)間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計(jì)的角度推導(dǎo)出了Kalman濾波器方程抹蚀。
目前剿牺，卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)】雒卡爾曼最初提出的形式現(xiàn)在一般稱(chēng)為簡(jiǎn)單卡爾曼濾波器牢贸。除此以外，還有施密特?cái)U(kuò)展濾波器镐捧、信息濾波器以及很多Bierman, Thornton開(kāi)發(fā)的平方根濾波器的變種潜索。也許最常見(jiàn)的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)懂酱、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛存在竹习。
從牛頓到卡爾曼
從現(xiàn)在開(kāi)始，就要進(jìn)行Kalman濾波器探討之旅了列牺，我們先回到高一整陌，從物理中小車(chē)的勻加速直線運(yùn)動(dòng)開(kāi)始。
話說(shuō)瞎领，有一輛質(zhì)量為m的小車(chē)泌辫，受恒定的力F，沿著r方向做勻加速直線運(yùn)動(dòng)九默。已知小車(chē)在t-ΔT時(shí)刻的位移是s(t-1)震放，此時(shí)的速度為v(t-1)。求：t時(shí)刻的位移是s(t)驼修，速度為v(t)殿遂？
由牛頓第二定律诈铛，求得加速度：
那么就有下面的位移和速度關(guān)系：
如果將上面的表達(dá)式用矩陣寫(xiě)在一起，就變成下面這樣：
卡爾曼濾波器是建立在動(dòng)態(tài)過(guò)程之上墨礁，由于物理量（位移幢竹，速度）的不可突變特性，這樣就可以通過(guò)t-1時(shí)刻估計(jì)（預(yù)測(cè)）t時(shí)刻的狀態(tài)恩静，其狀態(tài)空間模型為：
對(duì)比一下(1)(2)式焕毫，長(zhǎng)得及其相似有木有：
勻加速直線運(yùn)動(dòng)過(guò)程就是卡爾曼濾波中狀態(tài)空間模型的一個(gè)典型應(yīng)用。下面我們重點(diǎn)關(guān)注(2)式蜕企，鑒于研究的計(jì)算機(jī)信號(hào)都是離散的咬荷，將(2)是表示成離散形式為：
其中各個(gè)量之間的含義是：
x(n)是狀態(tài)向量，包含了觀測(cè)的目標(biāo)（如：位移轻掩、速度）
u(n)是驅(qū)動(dòng)輸入向量幸乒，如上面的運(yùn)動(dòng)過(guò)程是通過(guò)受力驅(qū)動(dòng)產(chǎn)生加速度，所以u(píng)(n)和受力有關(guān)
A是狀態(tài)轉(zhuǎn)移矩陣唇牧，其隱含指示了“n-1時(shí)刻的狀態(tài)會(huì)影響到n時(shí)刻的狀態(tài)（這似乎和馬爾可夫過(guò)程有些類(lèi)似）”
B是控制輸入矩陣罕扎，其隱含指示了“n時(shí)刻給的驅(qū)動(dòng)如何影響n時(shí)刻的狀態(tài)”
從運(yùn)動(dòng)的角度，很容易理解：小車(chē)當(dāng)前n時(shí)刻的位移和速度一部分來(lái)自于n-1時(shí)刻的慣性作用丐重，這通過(guò)Ax(n)來(lái)度量腔召，另一部分來(lái)自于現(xiàn)在n時(shí)刻小車(chē)新增加的外部受力，通過(guò)Bu(n)來(lái)度量扮惦。
w(n)是過(guò)程噪聲臀蛛，w(n)~N(0,Q)的高斯分布，過(guò)程噪聲是使用卡爾曼濾波器時(shí)一個(gè)重要的量崖蜜，后面會(huì)進(jìn)行分析浊仆。
計(jì)算n時(shí)刻的位移，還有一種方法：拿一把長(zhǎng)的卷尺（嗯豫领，如果小車(chē)跑了很長(zhǎng)時(shí)間抡柿，估計(jì)這把卷尺就難買(mǎi)到了），從起點(diǎn)一拉等恐，直接就出來(lái)了洲劣，設(shè)測(cè)量值為z(n)。計(jì)算速度呢课蔬？速度傳感器往那一用就出來(lái)了囱稽。
然而，初中物理就告訴我們二跋，“尺子是量不準(zhǔn)的战惊，物體的物理真實(shí)值無(wú)法獲得”，測(cè)量存在誤差同欠，我們暫且將這個(gè)誤差記為v(n)样傍。這種通過(guò)直接測(cè)量的方式獲得所需物理量的值構(gòu)成觀測(cè)空間：
z(n)就是測(cè)量結(jié)果，H(n)是觀測(cè)矢量铺遂，x(n)就是要求的物理量（位移衫哥、速度），v(n)~N(0,R)為測(cè)量噪聲襟锐，同狀態(tài)空間方程中的過(guò)程噪聲一樣撤逢，這也是一個(gè)后面要討論的量。大部分情況下粮坞，如果物理量能直接通過(guò)傳感器測(cè)量蚊荣，
現(xiàn)在就有了兩種方法（如上圖）可以得到n時(shí)刻的位移和速度：一種就是通過(guò)(3)式的狀態(tài)空間遞推計(jì)算（Prediction）莫杈，另一種就是通過(guò)(4)式直接拿尺子和傳感器測(cè)量（Measurement）互例。致命的是沒(méi)一個(gè)是精確無(wú)誤的，就像上圖看到的一樣筝闹，分別都存在0均值高斯分布的誤差w(n)和v(n)媳叨。
那么，我最終的結(jié)果是取尺子量出來(lái)的好呢关顷，還是根據(jù)我們偉大的牛頓第二定律推導(dǎo)出來(lái)的好呢糊秆？抑或兩者都不是！
一場(chǎng)遞推的游戲
為充分利用測(cè)量值（Measurement）和預(yù)測(cè)值（Prediction）议双，Kalman濾波并不是簡(jiǎn)單的取其中一個(gè)作為輸出痘番，也不是求平均。
設(shè)預(yù)測(cè)過(guò)程噪聲w(n)N(0,Q)平痰，測(cè)量噪聲v(n)N(0,R)汞舱。Kalman計(jì)算輸出分為預(yù)測(cè)過(guò)程和修正過(guò)程如下：
預(yù)測(cè)
預(yù)測(cè)值：
最小均方誤差矩陣：
修正
誤差增益：
修正值：
最小均方誤差矩陣：
從(5)~(9)中：
x(n)：Nx1的狀態(tài)矢量
z(n)：Mx1的觀測(cè)矢量觉增，Kalman濾波器的輸入
x(n|n-1)：用n時(shí)刻以前的數(shù)據(jù)進(jìn)行對(duì)n時(shí)刻的估計(jì)結(jié)果
x(n|n)：用n時(shí)刻及n時(shí)刻以前的數(shù)據(jù)對(duì)n時(shí)刻的估計(jì)結(jié)果兵拢，這也是Kalman濾波器的輸出
P(n|n-1)：NxN，最小預(yù)測(cè)均方誤差矩陣逾礁，其定義式為
通過(guò)計(jì)算最終得到(6)式说铃。
P(n|n)：NxN，修正后最小均方誤差矩陣嘹履。
K(n)：NxM腻扇，誤差增益，從增益的表達(dá)式看砾嫉，相當(dāng)于“預(yù)測(cè)最小均方誤差”除以“n時(shí)刻的測(cè)量誤差+預(yù)測(cè)最小均方誤差”幼苛，直觀含義就是用n-1預(yù)測(cè)n時(shí)刻狀態(tài)的預(yù)測(cè)最小均方誤差在n時(shí)刻的總誤差中的比重，比重越大焕刮，說(shuō)明真值接近預(yù)測(cè)值的概率越胁把亍（接近測(cè)量值的概率越大）墙杯，這也可以從(8)式中看到。
Kalman濾波算法的步驟是(5)(6)->(7)->(8)(9)括荡。當(dāng)然高镐，建議找本教材復(fù)習(xí)下上面公式的推導(dǎo)過(guò)程，或參見(jiàn)Wiki上的介紹http://en.wikipedia.org/wiki/Kalman_filter畸冲。
公式就是那么的抽象嫉髓，一旦認(rèn)真研究懂了卻也是茅塞頓開(kāi)，受益也比只知皮毛的多邑闲。盡管如此算行，我還算更喜歡先感性后理性。仍以上面的運(yùn)動(dòng)的例子來(lái)直觀分析：
Example:
還可以更簡(jiǎn)單一些：設(shè)小車(chē)做勻速（而非勻加速）直線運(yùn)動(dòng)苫耸，方便計(jì)算州邢，假設(shè)速度絕對(duì)的恒定（不波動(dòng)，所以相關(guān)的方差都為0）褪子，則u(t)==0恒成立偷霉。設(shè)預(yù)測(cè)（過(guò)程）位移噪聲w(n)N(0,2^2)，測(cè)量位移噪聲v(n)N(0,1^2)褐筛，n-1狀態(tài)的位移
則A = [1 ΔT; 0 1]=[1 1; 0 1]残吩，根據(jù)(5)，預(yù)測(cè)值為
現(xiàn)在已經(jīng)有了估計(jì)值和測(cè)量值泣侮，哪個(gè)更接近真值，這就通過(guò)最小均方誤差矩陣來(lái)決定紧唱！
要求已知上次的修正后的最小均方誤差P(n-1|n-1)=[1 0; 0 0]（勻速活尊，所以P(2,2)=0，右斜對(duì)角線上為協(xié)方差漏益，值為0蛹锰，P(1,1)為n-1時(shí)刻位移量的均方誤差，因?yàn)橐?jì)算P(1,1)還得先遞推往前計(jì)算P(n-2|n-2)绰疤，所以這里暫時(shí)假設(shè)為1）铜犬，則根據(jù)(6)式，最小預(yù)測(cè)預(yù)測(cè)均方誤差為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時(shí)刻估計(jì)值既不是用n-1得到的估計(jì)值攘蔽，也不是測(cè)量值，而是：
從上面的遞推關(guān)系知道作岖，要估計(jì)n時(shí)刻就必須知道n-1時(shí)刻，那么n=0時(shí)刻該如何估計(jì)五芝，因此痘儡，卡爾曼濾波要初始化的估計(jì)值x(-1|-1)和誤差矩陣P(-1|-1)，設(shè)x(-1,-1)~N(Us, Cs)枢步，則初始化：
綜上沉删，借用一張圖說(shuō)明一下Kalman濾波算法的流程：
圖中的符號(hào)和本文符號(hào)稍有差異，主要是P的表示上醉途。從上圖也可以看出矾瑰，Kalman濾波就是給定-1時(shí)刻的初始值，然后在預(yù)測(cè)（狀態(tài)空間）和修正（觀測(cè)空間）之間不停的遞推隘擎，求取n時(shí)刻的估計(jì)x和均方誤差矩陣P殴穴。
均方誤差中的門(mén)道
到這里，應(yīng)該對(duì)Kalman濾波有個(gè)總體的概念了货葬，有幾個(gè)觀點(diǎn)很重要采幌，是建立Kalman濾波器的基礎(chǔ)：
一個(gè)是n-1對(duì)n時(shí)刻估計(jì)值，一個(gè)是n時(shí)刻的測(cè)量值震桶，估計(jì)值和測(cè)量值都存在誤差休傍，且誤差都假設(shè)滿足獨(dú)立的高斯分布
Kalman濾波器就是充分結(jié)合了估計(jì)值和測(cè)量值得到n時(shí)刻更接近真值的估計(jì)結(jié)果
Kalman濾波器引入狀態(tài)空間的目的是避免了“像Wiener濾波器一樣需要對(duì)過(guò)去所有[0,n-1]時(shí)刻協(xié)方差先驗(yàn)知識(shí)都已知”，而直接可以通過(guò)上一時(shí)刻即n-1時(shí)刻的狀態(tài)信息和均方誤差信息就可遞推得到n時(shí)刻的估計(jì)蹲姐。盡管遞推使得實(shí)際應(yīng)用中方便了磨取，但n-1對(duì)n時(shí)刻的估計(jì)實(shí)際上使用到了所有前[0,n-1]時(shí)刻的信息，只不過(guò)信息一直通過(guò)最小均方誤差進(jìn)行傳遞到n-1時(shí)刻柴墩∏奚溃基于此，Kalman濾波也需要先驗(yàn)知識(shí)拐邪，即-1時(shí)刻的初始值慰毅。
在上小節(jié)中只看到Kalman的結(jié)論，那么Kalman濾波器是如何將估計(jì)值和測(cè)量值結(jié)合起來(lái)扎阶，如何將信息傳遞下去的呢汹胃？這其中婶芭，“獨(dú)立高斯分布”的假設(shè)條件功勞不可謂不大！測(cè)量值z(mì)(n)N(uz,σz^2)着饥，估計(jì)值x(n)N(ux,σx^2)犀农。
Kalman濾波器巧妙的用“獨(dú)立高斯分布的乘積”將這兩個(gè)測(cè)量值和估計(jì)值進(jìn)行融合！
如下圖：估計(jì)量的高斯分布和測(cè)量量的高斯分布經(jīng)過(guò)融合后為綠色的高斯分布曲線宰掉。
稍微計(jì)算一下呵哨，通過(guò)上式求出u和σ^2，
現(xiàn)在令
則(10)(11)變成：
到這里轨奄，請(qǐng)將(13)-(14)與(8)-(9)式對(duì)比孟害！標(biāo)量的情況下，在小車(chē)的應(yīng)用中有：A=1挪拟，H=1挨务，正態(tài)分布的均值u就是我們要的輸出結(jié)果，正態(tài)分布的方差σz^2就是最小均方誤差玉组。推廣到矢量的情況谎柄，最小均方誤差矩陣就是多維正態(tài)分布的協(xié)方差矩陣。
從(12)式也很容易看到卡爾曼增益K的含義：就是估計(jì)量的方差占總方差（包括估計(jì)方差和測(cè)量方差）的比重惯雳。
一切都變得晴朗起來(lái)了朝巫，然而這一切的一切，卻都源自于“估計(jì)量和測(cè)量量的獨(dú)立高斯分布”這條假設(shè)石景。進(jìn)一步總結(jié)Kalman濾波器：
假設(shè)狀態(tài)空間的n-1時(shí)刻估計(jì)值和觀測(cè)空間的n時(shí)刻測(cè)量值都滿足獨(dú)立高斯分布捍歪，Kalman濾波器就是通過(guò)高斯分布的乘積運(yùn)算將估計(jì)值和測(cè)量值結(jié)合，獲得最接近真值的n時(shí)刻估計(jì)鸵钝。
高斯分布乘積運(yùn)算的結(jié)果仍為高斯分布糙臼，高斯分布的均值對(duì)應(yīng)n時(shí)刻的估計(jì)值，高斯分布的方差對(duì)應(yīng)n時(shí)刻的均方誤差恩商。
Matlab程序看過(guò)來(lái)
下面的一段Matlab代碼是從網(wǎng)上找到的变逃，程序簡(jiǎn)單直接，但作為學(xué)習(xí)分析用很棒
該程序中使用的符號(hào)的含義與本文一致怠堪，函數(shù)前的注釋再清晰不過(guò)了揽乱，就不多說(shuō)。下面是一段[測(cè)試]
Kalman的參數(shù)中s.Q和s.R的設(shè)置非常重要粟矿，之前也提過(guò)凰棉，一般要通過(guò)實(shí)驗(yàn)統(tǒng)計(jì)得到，它們分布代表了狀態(tài)空間估計(jì)的誤差和測(cè)量的誤差陌粹。
Kalman濾波器的效果是使輸出變得更平滑撒犀，但沒(méi)辦法去除信號(hào)中原有的椒鹽噪聲，而且，Kalman濾波器也會(huì)跟蹤這些椒鹽噪聲點(diǎn)或舞，因此推薦在使用Kalman濾波器前先使用中值濾波去除椒鹽噪聲荆姆。
Kalman濾波C程序
我就在上面公式的基礎(chǔ)上實(shí)現(xiàn)了基本的Kalman濾波器，包括1維和2維狀態(tài)的情況映凳。先在頭文件中聲明1維和2維Kalman濾波器結(jié)構(gòu)：
我都給了有詳細(xì)的注釋?zhuān)琸alman1_state
是狀態(tài)空間為1維/測(cè)量空間1維的Kalman濾波器胆筒，kalman2_state
是狀態(tài)空間為2維/測(cè)量空間1維的Kalman濾波器。兩個(gè)結(jié)構(gòu)體都需要通過(guò)初始化函數(shù)初始化相關(guān)參數(shù)诈豌、狀態(tài)值和均方差值仆救。
其實(shí)，Kalman濾波器由于其遞推特性矫渔，實(shí)現(xiàn)起來(lái)很簡(jiǎn)單彤蔽。但調(diào)參有很多可研究的地方，主要需要設(shè)定的參數(shù)如下：
init_x：待測(cè)量的初始值蚌斩，如有中值一般設(shè)成中值（如陀螺儀）
init_p：后驗(yàn)狀態(tài)估計(jì)值誤差的方差的初始值
q：預(yù)測(cè)（過(guò)程）噪聲方差
r：測(cè)量（觀測(cè)）噪聲方差。以陀螺儀為例范嘱，測(cè)試方法是：保持陀螺儀不動(dòng)送膳，統(tǒng)計(jì)一段時(shí)間內(nèi)的陀螺儀輸出數(shù)據(jù)。數(shù)據(jù)會(huì)近似正態(tài)分布丑蛤，按3σ原則叠聋，取正態(tài)分布的(3σ)^2作為r的初始化值。
其中q和r參數(shù)尤為重要受裹，一般得通過(guò)實(shí)驗(yàn)測(cè)試得到碌补。
找兩組聲陣列測(cè)向的角度數(shù)據(jù)，對(duì)上面的C程序進(jìn)行測(cè)試棉饶。一維Kalman（一維也是標(biāo)量的情況厦章，就我所知，現(xiàn)在網(wǎng)上看到的代碼大都是使用標(biāo)量的情況）和二維Kalman（一個(gè)狀態(tài)是角度值照藻，另一個(gè)狀態(tài)是向量角度差袜啃，也就是角速度）的結(jié)果都在圖中顯示。這里再稍微提醒一下：狀態(tài)量不要取那些能突變的量幸缕，如加速度群发，這點(diǎn)在文章“從牛頓到卡爾曼”一小節(jié)就提到過(guò)。
Matlab繪出的跟蹤結(jié)果顯示：
Kalman濾波結(jié)果比原信號(hào)更平滑发乔。但是有椒鹽突變?cè)肼暤牡胤绞旒耍琄alman濾波器并不能濾除椒鹽噪聲的影響，也會(huì)跟蹤椒鹽噪聲點(diǎn)栏尚。因此起愈，推薦在Kalman濾波器之前先使用中值濾波算法去除椒鹽突變點(diǎn)的影響。
上面所有C程序的源代碼及測(cè)試程序都公布在我的Github上，希望大家批評(píng)指正其中可能存在的錯(cuò)誤告材。
