你的位置:首頁 > RF/微波 > 正文

深度解讀:卡爾曼濾波原理

發(fā)布時(shí)間:2019-07-29 責(zé)任編輯:wenwei

【導(dǎo)讀】在網(wǎng)上看了不少與卡爾曼濾波相關(guān)的博客、論文,要么是只談理論、缺乏感性,或者有感性認(rèn)識(shí),缺乏理論推導(dǎo)。能兼顧二者的少之又少,直到我看到了國外的一篇博文,真的驚艷到我了,不得不佩服作者這種細(xì)致入微的精神,翻譯過來跟大家分享一下。
 
我不得不說說卡爾曼濾波,因?yàn)樗茏龅降氖虑楹喼弊屓梭@嘆!意外的是很少有軟件工程師和科學(xué)家對(duì)對(duì)它有所了解,這讓我感到沮喪,因?yàn)榭柭鼮V波是一個(gè)如此強(qiáng)大的工具,能夠在不確定性中融合信息,與此同時(shí),它提取精確信息的能力看起來不可思議。
 
什么是卡爾曼濾波?
 
你可以在任何含有不確定信息的動(dòng)態(tài)系統(tǒng)中使用卡爾曼濾波,對(duì)系統(tǒng)下一步的走向做出有根據(jù)的預(yù)測,即使伴隨著各種干擾,卡爾曼濾波總是能指出真實(shí)發(fā)生的情況。
 
在連續(xù)變化的系統(tǒng)中使用卡爾曼濾波是非常理想的,它具有占用內(nèi)存小的優(yōu)點(diǎn)(除了前一個(gè)狀態(tài)量外,不需要保留其它歷史數(shù)據(jù)),并且速度很快,很適合應(yīng)用于實(shí)時(shí)問題和嵌入式系統(tǒng)。
 
在Google上找到的大多數(shù)關(guān)于實(shí)現(xiàn)卡爾曼濾波的數(shù)學(xué)公式看起來有點(diǎn)晦澀難懂,這個(gè)狀況有點(diǎn)糟糕。實(shí)際上,如果以正確的方式看待它,卡爾曼濾波是非常簡單和容易理解的,下面我將用漂亮的圖片和色彩清晰的闡述它,你只需要懂一些基本的概率和矩陣的知識(shí)就可以了。
 
我們能用卡爾曼濾波做什么?
 
用玩具舉例:你開發(fā)了一個(gè)可以在樹林里到處跑的小機(jī)器人,這個(gè)機(jī)器人需要知道它所在的確切位置才能導(dǎo)航。
 
深度解讀:卡爾曼濾波原理
 
我們可以說機(jī)器人有一個(gè)狀態(tài)深度解讀:卡爾曼濾波原理,表示位置和速度:
 
深度解讀:卡爾曼濾波原理
 
注意這個(gè)狀態(tài)只是關(guān)于這個(gè)系統(tǒng)基本屬性的一堆數(shù)字,它可以是任何其它的東西。在這個(gè)例子中是位置和速度,它也可以是一個(gè)容器中液體的總量,汽車發(fā)動(dòng)機(jī)的溫度,用戶手指在觸摸板上的位置坐標(biāo),或者任何你需要跟蹤的信號(hào)。
 
這個(gè)機(jī)器人帶有GPS,精度大約為10米,還算不錯(cuò),但是,它需要將自己的位置精確到10米以內(nèi)。樹林里有很多溝壑和懸崖,如果機(jī)器人走錯(cuò)了一步,就有可能掉下懸崖,所以只有GPS是不夠的。
 
深度解讀:卡爾曼濾波原理
 
或許我們知道一些機(jī)器人如何運(yùn)動(dòng)的信息:例如,機(jī)器人知道發(fā)送給電機(jī)的指令,知道自己是否在朝一個(gè)方向移動(dòng)并且沒有人干預(yù),在下一個(gè)狀態(tài),機(jī)器人很可能朝著相同的方向移動(dòng)。當(dāng)然,機(jī)器人對(duì)自己的運(yùn)動(dòng)是一無所知的:它可能受到風(fēng)吹的影響,輪子方向偏了一點(diǎn),或者遇到不平的地面而翻倒。所以,輪子轉(zhuǎn)過的長度并不能精確表示機(jī)器人實(shí)際行走的距離,預(yù)測也不是很完美。
 
GPS 傳感器告訴了我們一些狀態(tài)信息,我們的預(yù)測告訴了我們機(jī)器人會(huì)怎樣運(yùn)動(dòng),但都只是間接的,并且伴隨著一些不確定和不準(zhǔn)確性。但是,如果使用所有對(duì)我們可用的信息,我們能得到一個(gè)比任何依據(jù)自身估計(jì)更好的結(jié)果嗎?回答當(dāng)然是YES,這就是卡爾曼濾波的用處。
 
卡爾曼濾波是如何看到你的問題的
 
下面我們繼續(xù)以只有位置和速度這兩個(gè)狀態(tài)的簡單例子做解釋。
 
深度解讀:卡爾曼濾波原理
 
我們并不知道實(shí)際的位置和速度,它們之間有很多種可能正確的組合,但其中一些的可能性要大于其它部分:
 
深度解讀:卡爾曼濾波原理
 
卡爾曼濾波假設(shè)兩個(gè)變量(位置和速度,在這個(gè)例子中)都是隨機(jī)的,并且服從高斯分布。每個(gè)變量都有一個(gè)均值 μ,表示隨機(jī)分布的中心(最可能的狀態(tài)),以及方差深度解讀:卡爾曼濾波原理,表示不確定性。
 
深度解讀:卡爾曼濾波原理
 
在上圖中,位置和速度是不相關(guān)的,這意味著由其中一個(gè)變量的狀態(tài)無法推測出另一個(gè)變量可能的值。下面的例子更有趣:位置和速度是相關(guān)的,觀測特定位置的可能性取決于當(dāng)前的速度:
 
深度解讀:卡爾曼濾波原理
 
這種情況是有可能發(fā)生的,例如,我們基于舊的位置來估計(jì)新位置。如果速度過高,我們可能已經(jīng)移動(dòng)很遠(yuǎn)了。如果緩慢移動(dòng),則距離不會(huì)很遠(yuǎn)。跟蹤這種關(guān)系是非常重要的,因?yàn)樗鼛Ыo我們更多的信息:其中一個(gè)測量值告訴了我們其它變量可能的值,這就是卡爾曼濾波的目的,盡可能地在包含不確定性的測量數(shù)據(jù)中提取更多信息!
 
這種相關(guān)性用協(xié)方差矩陣來表示,簡而言之,矩陣中的每個(gè)元素深度解讀:卡爾曼濾波原理表示第 i 個(gè)和第 j 個(gè)狀態(tài)變量之間的相關(guān)度。(你可能已經(jīng)猜到協(xié)方差矩陣是一個(gè)對(duì)稱矩陣,這意味著可以任意交換 i 和 j)。協(xié)方差矩陣通常用“深度解讀:卡爾曼濾波原理”來表示,其中的元素則表示為“深度解讀:卡爾曼濾波原理”。
 
深度解讀:卡爾曼濾波原理
 
使用矩陣來描述問題
 
我們基于高斯分布來建立狀態(tài)變量,所以在時(shí)刻 k 需要兩個(gè)信息:最佳估計(jì)深度解讀:卡爾曼濾波原理(即均值,其它地方常用 μ 表示),以及協(xié)方差矩陣深度解讀:卡爾曼濾波原理
 
深度解讀:卡爾曼濾波原理     (1)
 
(當(dāng)然,在這里我們只用到了位置和速度,實(shí)際上這個(gè)狀態(tài)可以包含多個(gè)變量,代表任何你想表示的信息)。接下來,我們需要根據(jù)當(dāng)前狀態(tài)(k-1 時(shí)刻)來預(yù)測下一狀態(tài)(k 時(shí)刻)。記住,我們并不知道對(duì)下一狀態(tài)的所有預(yù)測中哪個(gè)是“真實(shí)”的,但我們的預(yù)測函數(shù)并不在乎。它對(duì)所有的可能性進(jìn)行預(yù)測,并給出新的高斯分布。
 
深度解讀:卡爾曼濾波原理
 
我們可以用矩陣深度解讀:卡爾曼濾波原理來表示這個(gè)預(yù)測過程:
 
深度解讀:卡爾曼濾波原理
 
它將我們?cè)脊烙?jì)中的每個(gè)點(diǎn)都移動(dòng)到了一個(gè)新的預(yù)測位置,如果原始估計(jì)是正確的話,這個(gè)新的預(yù)測位置就是系統(tǒng)下一步會(huì)移動(dòng)到的位置。那我們又如何用矩陣來預(yù)測下一個(gè)時(shí)刻的位置和速度呢?下面用一個(gè)基本的運(yùn)動(dòng)學(xué)公式來表示:
 
深度解讀:卡爾曼濾波原理
 
現(xiàn)在,我們有了一個(gè)預(yù)測矩陣來表示下一時(shí)刻的狀態(tài),但是,我們?nèi)匀徊恢涝趺锤聟f(xié)方差矩陣。此時(shí),我們需要引入另一個(gè)公式,如果我們將分布中的每個(gè)點(diǎn)都乘以矩陣 A,那么它的協(xié)方差矩陣深度解讀:卡爾曼濾波原理會(huì)怎樣變化呢?很簡單,下面給出公式:
 
深度解讀:卡爾曼濾波原理
 
結(jié)合方程(4)和(3)得到:
 
深度解讀:卡爾曼濾波原理
 
外部控制量
 
我們并沒有捕捉到一切信息,可能存在外部因素會(huì)對(duì)系統(tǒng)進(jìn)行控制,帶來一些與系統(tǒng)自身狀態(tài)沒有相關(guān)性的改變。
 
以火車的運(yùn)動(dòng)狀態(tài)模型為例,火車司機(jī)可能會(huì)操縱油門,讓火車加速。相同地,在我們機(jī)器人這個(gè)例子中,導(dǎo)航軟件可能會(huì)發(fā)出一個(gè)指令讓輪子轉(zhuǎn)向或者停止。如果知道這些額外的信息,我們可以用一個(gè)向量深度解讀:卡爾曼濾波原理來表示,將它加到我們的預(yù)測方程中做修正。
 
假設(shè)由于油門的設(shè)置或控制命令,我們知道了期望的加速度深度解讀:卡爾曼濾波原理,根據(jù)基本的運(yùn)動(dòng)學(xué)方程可以得到:
 
深度解讀:卡爾曼濾波原理
 
以矩陣的形式表示就是:
 
深度解讀:卡爾曼濾波原理
 
深度解讀:卡爾曼濾波原理稱為控制矩陣,深度解讀:卡爾曼濾波原理稱為控制向量(對(duì)于沒有外部控制的簡單系統(tǒng)來說,這部分可以忽略)。讓我們?cè)偎伎家幌?,如果我們的預(yù)測并不是100%準(zhǔn)確的,該怎么辦呢?
 
外部干擾
 
如果這些狀態(tài)量是基于系統(tǒng)自身的屬性或者已知的外部控制作用來變化的,則不會(huì)出現(xiàn)什么問題。
 
但是,如果存在未知的干擾呢?例如,假設(shè)我們跟蹤一個(gè)四旋翼飛行器,它可能會(huì)受到風(fēng)的干擾,如果我們跟蹤一個(gè)輪式機(jī)器人,輪子可能會(huì)打滑,或者路面上的小坡會(huì)讓它減速。這樣的話我們就不能繼續(xù)對(duì)這些狀態(tài)進(jìn)行跟蹤,如果沒有把這些外部干擾考慮在內(nèi),我們的預(yù)測就會(huì)出現(xiàn)偏差。
 
在每次預(yù)測之后,我們可以添加一些新的不確定性來建立這種與“外界”(即我們沒有跟蹤的干擾)之間的不確定性模型:
 
深度解讀:卡爾曼濾波原理
 
原始估計(jì)中的每個(gè)狀態(tài)變量更新到新的狀態(tài)后,仍然服從高斯分布。我們可以說深度解讀:卡爾曼濾波原理的每個(gè)狀態(tài)變量移動(dòng)到了一個(gè)新的服從高斯分布的區(qū)域,協(xié)方差為深度解讀:卡爾曼濾波原理。換句話說就是,我們將這些沒有被跟蹤的干擾當(dāng)作協(xié)方差為深度解讀:卡爾曼濾波原理的噪聲來處理。
 
深度解讀:卡爾曼濾波原理
 
這產(chǎn)生了具有不同協(xié)方差(但是具有相同的均值)的新的高斯分布。
 
深度解讀:卡爾曼濾波原理
 
我們通過簡單地添加深度解讀:卡爾曼濾波原理得到擴(kuò)展的協(xié)方差,下面給出預(yù)測步驟的完整表達(dá)式:
 
深度解讀:卡爾曼濾波原理
 
由上式可知,新的最優(yōu)估計(jì)是根據(jù)上一最優(yōu)估計(jì)預(yù)測得到的,并加上已知外部控制量的修正。
 
而新的不確定性由上一不確定性預(yù)測得到,并加上外部環(huán)境的干擾。
 
好了,我們對(duì)系統(tǒng)可能的動(dòng)向有了一個(gè)模糊的估計(jì),用深度解讀:卡爾曼濾波原理深度解讀:卡爾曼濾波原理來表示。如果再結(jié)合傳感器的數(shù)據(jù)會(huì)怎樣呢?
 
用測量值來修正估計(jì)值
 
我們可能會(huì)有多個(gè)傳感器來測量系統(tǒng)當(dāng)前的狀態(tài),哪個(gè)傳感器具體測量的是哪個(gè)狀態(tài)變量并不重要,也許一個(gè)是測量位置,一個(gè)是測量速度,每個(gè)傳感器間接地告訴了我們一些狀態(tài)信息。
 
深度解讀:卡爾曼濾波原理
 
注意,傳感器讀取的數(shù)據(jù)的單位和尺度有可能與我們要跟蹤的狀態(tài)的單位和尺度不一樣,我們用矩陣深度解讀:卡爾曼濾波原理來表示傳感器的數(shù)據(jù)。
 
深度解讀:卡爾曼濾波原理
 
我們可以計(jì)算出傳感器讀數(shù)的分布,用之前的表示方法如下式所示:
 
深度解讀:卡爾曼濾波原理
 
卡爾曼濾波的一大優(yōu)點(diǎn)就是能處理傳感器噪聲,換句話說,我們的傳感器或多或少都有點(diǎn)不可靠,并且原始估計(jì)中的每個(gè)狀態(tài)可以和一定范圍內(nèi)的傳感器讀數(shù)對(duì)應(yīng)起來。
 
深度解讀:卡爾曼濾波原理
 
從測量到的傳感器數(shù)據(jù)中,我們大致能猜到系統(tǒng)當(dāng)前處于什么狀態(tài)。但是由于存在不確定性,某些狀態(tài)可能比我們得到的讀數(shù)更接近真實(shí)狀態(tài)。
 
深度解讀:卡爾曼濾波原理
 
我們將這種不確定性(例如:傳感器噪聲)用協(xié)方差深度解讀:卡爾曼濾波原理表示,該分布的均值就是我們讀取到的傳感器數(shù)據(jù),稱之為深度解讀:卡爾曼濾波原理。
 
現(xiàn)在我們有了兩個(gè)高斯分布,一個(gè)是在預(yù)測值附近,一個(gè)是在傳感器讀數(shù)附近。
 
深度解讀:卡爾曼濾波原理
 
我們必須在預(yù)測值(粉紅色)和傳感器測量值(綠色)之間找到最優(yōu)解。
 
那么,我們最有可能的狀態(tài)是什么呢?對(duì)于任何可能的讀數(shù)深度解讀:卡爾曼濾波原理,有兩種情況:(1)傳感器的測量值;(2)由前一狀態(tài)得到的預(yù)測值。如果我們想知道這兩種情況都可能發(fā)生的概率,將這兩個(gè)高斯分布相乘就可以了。
 
深度解讀:卡爾曼濾波原理
 
剩下的就是重疊部分了,這個(gè)重疊部分的均值就是兩個(gè)估計(jì)最可能的值,也就是給定的所有信息中的最優(yōu)估計(jì)。
 
瞧!這個(gè)重疊的區(qū)域看起來像另一個(gè)高斯分布。
 
深度解讀:卡爾曼濾波原理
 
如你所見,把兩個(gè)具有不同均值和方差的高斯分布相乘,你會(huì)得到一個(gè)新的具有獨(dú)立均值和方差的高斯分布!下面用公式講解。
 
融合高斯分布
 
先以一維高斯分布來分析比較簡單點(diǎn),具有方差深度解讀:卡爾曼濾波原理和 μ 的高斯曲線可以用下式表示:
 
深度解讀:卡爾曼濾波原理
 
如果把兩個(gè)服從高斯分布的函數(shù)相乘會(huì)得到什么呢?
 
深度解讀:卡爾曼濾波原理
 
將式(9)代入到式(10)中(注意重新歸一化,使總概率為1)可以得到:
 
深度解讀:卡爾曼濾波原理
 
將式(11)中的兩個(gè)式子相同的部分用 k 表示:
 
深度解讀:卡爾曼濾波原理
 
下面進(jìn)一步將式(12)和(13)寫成矩陣的形式,如果 Σ 表示高斯分布的協(xié)方差,深度解讀:卡爾曼濾波原理表示每個(gè)維度的均值,則:
 
深度解讀:卡爾曼濾波原理
 
矩陣深度解讀:卡爾曼濾波原理稱為卡爾曼增益,下面將會(huì)用到。放松!我們快要完成了!
 
將所有公式整合起來
 
我們有兩個(gè)高斯分布,預(yù)測部分深度解讀:卡爾曼濾波原理,和測量部分深度解讀:卡爾曼濾波原理,將它們放到式(15)中算出它們之間的重疊部分:
 
深度解讀:卡爾曼濾波原理
 
由式(14)可得卡爾曼增益為:
 
深度解讀:卡爾曼濾波原理
 
將式(16)和式(17)的兩邊同時(shí)左乘矩陣的逆(注意深度解讀:卡爾曼濾波原理里面包含了深度解讀:卡爾曼濾波原理)將其約掉,再將式(16)的第二個(gè)等式兩邊同時(shí)右乘矩陣深度解讀:卡爾曼濾波原理的逆得到以下等式:
 
深度解讀:卡爾曼濾波原理
 
上式給出了完整的更新步驟方程。深度解讀:卡爾曼濾波原理就是新的最優(yōu)估計(jì),我們可以將它和深度解讀:卡爾曼濾波原理放到下一個(gè)預(yù)測和更新方程中不斷迭代。
 
深度解讀:卡爾曼濾波原理
 
總結(jié)
 
以上所有公式中,你只需要用到式(7)、(18)、(19)。(如果忘了的話,你可以根據(jù)式(4)和(15)重新推導(dǎo)一下)
 
我們可以用這些公式對(duì)任何線性系統(tǒng)建立精確的模型,對(duì)于非線性系統(tǒng)來說,我們使用擴(kuò)展卡爾曼濾波,區(qū)別在于EKF多了一個(gè)把預(yù)測和測量部分進(jìn)行線性化的過程。
 
本文轉(zhuǎn)載自電子工程專輯。
 
 
推薦閱讀:
 
為什么采用4~20mA的電流來傳輸模擬量?
羅姆榮獲歌爾頒發(fā)的聯(lián)合創(chuàng)新獎(jiǎng)
超聲系統(tǒng)的信號(hào)鏈設(shè)計(jì)注意事項(xiàng)
慶祝人類登月50周年:TI集成電路的一大步
邊緣智能傳感可實(shí)現(xiàn)更智能的自主機(jī)器人
特別推薦
技術(shù)文章更多>>
技術(shù)白皮書下載更多>>
熱門搜索
?

關(guān)閉

?

關(guān)閉