卡爾曼濾波器推導與解析 - 案例與圖片
隨筆- 5 文章- 2 評論- 13
</div>
<div id="mylinks">
卡爾曼濾波器推導與解析 - 案例與圖片
在SLAM的後端優化中有存在這兩大類優化方法:濾波器優化和非線性優化.目前大多數基於視覺的SLAM演算法都是採用的非線性優化的相關方法(如應用較多的G2O圖優化框架).但是濾波器的方法仍然在某些情況下有應用,且以前不少論文都是基於濾波器優化的方法設計的SLAM演算法.因此學習濾波器優化的方法對於做SLAM演算法相關的研究還是有很大的意義的.
卡爾曼濾波(KF)和擴充套件卡爾曼濾波(EKF)是一類常用的濾波器優化方法.本文主要翻譯一片國外的部落格,以較為易懂的方式介紹卡爾曼濾波器的結構以及背後的數學原理.如果各位英文不錯還是比較推薦直接看英文原文,部落格的原地址為:http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mathybits
除了這篇文章以外個人認為以下這幾篇文章也寫的非常好,推薦閱讀:
1.
這篇文章通俗易懂,還配有大量的互動式圖形小程式幫助大家理解各個引數對整個系統的影響,非常適合入門.同時該文章還配有matlab程式碼和C++程式碼.
2. Tutorial: The Kalman Filter - MIT
這是一篇MIT課程上的tutorial.比較深入的講解了KF濾波器背後的一些原理.
3. KF, EKF and SLAM course in MATLAB
該課程授課教師為MATLAB SLAM TOOLBOX的作者。在該課程上詳細講解了KF和EKF的相關理論以及EKF-SLAM的實現方法,對於大家深入瞭解EKF以及EKFSLAM相關理論(稀疏矩陣,反向深度,反向模型)都有很大幫助(附帶KF、EKF以及EKFSLAM Matlab程式碼以及對應視訊)。
4. Probabilistic Robotics - Chpter 3
最後強烈推薦一下概率機器人,裡面的卡爾曼濾波這一章講的非常的好,如果各位有時間強烈建議讀一下.
以下為翻譯內容(略有更改) - How a Kalman filter works, in picture
------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
1. 使用卡爾曼濾波器的目的
我們假設你建造了一個可以在樹林裡行走的小機器人,為了精準的進行導航,機器人需要每時每刻都知道它自己的位置
我們用符號來表示機器人的狀態變數,在此處我們假設狀態變數只包含機器人的位置和速度:
需要注意的是,狀態只是一列和你係統有關的變數,它可以是任何的變數,不僅限於位置和速度(例如可以將引擎的溫度,操作人員的手指在觸控板上的位置等變數作為系統的狀態變數,只要這些變數的狀態是可跟蹤的).
在製造這個機器人的時候,我們在它上面也安裝了一些感測器,其中就包括GPS.機器人上的GPSD定位精度為10m,但是由於樹林中存在一些障礙物以及窪地,懸崖我們需求更高精度的定位能力,否則機器人就有可能撞到障礙物或者是跌落.
同時,我們也知道一些與機器人運動相關的資訊(變數):知道控制埠傳送至機器人驅動輪上的命令,知道機器人的正方向朝向以及前方有無障礙物等.但是很明顯,我們無法獲取到有關機器人運動的所有資訊(變數):機器人有可能收到強風影響,驅動輪可能打滑等.因此僅僅是通過計量驅動輪轉動圈數無法準確的計算出機器人走了有多遠.
同理,GPS能夠非直接的告訴我們一些機器人的狀態,但是GPS所提供的資訊也是帶有誤差和不確定行的.因此如果只是依據GPS做出的預測也是不夠精確的.
雖然單一的依靠某一種資訊無法給我們提供一個具有足夠置信度的結果,但是如果我們將所有有用的資訊結合在一起,我們能否在不依賴機器人自身以外的感測器得到一個比較的預測結果呢?答案當然是肯定的,這就是卡爾曼濾波器的設計目的.
2. 卡爾曼濾波器
同上,我們將繼續用位置和速度來代表機器人的狀態:
由於不確定性的存在,機器人當前時刻的位置和速度值存在很多可能的可能性,我們無法知道準確的值.但是某中可能性的概率相較於其他會更大,如下圖所示:
卡爾曼濾波器假設所有變數(在此例中為位置和速度)都滿足隨機的高斯分佈.每一個變數都有均值u來代表隨機分佈的中心點,符號代表了不確定性.關於這兩個變數的解釋如下圖所示:
在該示意圖中,位置和速度是不具有關聯性的,也就是說我們無法通過位置推斷得到和與速度有關的資訊,反之亦然.
當位置和速度變數具有關聯性時,其示意圖如下所示.如圖所示,觀察到某一個特定位置的可能性(概率)受當前機器人的速度影響.
上圖所述這種情形可能在這種情況下發生.例如,當前我們依據上一時刻的位置去預測機器人下一時刻的位置.如果機器人的執行速度較高,其移動距離也可能變的更大,從而導致計算的位置也發生較大的距離變化.反之亦然.
這種關係是十分重要的,因為它會告訴我們更多的有關資訊:一個變數的狀態將會反應出其他變數量的一些狀態.這也是卡爾曼濾波器的目的,即我們希望儘量的從具有不確定性的資訊中獲取儘可能多的有用資訊.
在之後計算過程中我們用協方差矩陣(covariance matrix)來代表和描述這種關聯關係.即Σij描述了第i個狀態變數與第j個狀態變數之間的關聯性.協方差矩陣通常用符號Σ表示,其中的元素表示為Σij.
3. 用矩陣方式表述問題
現在我們運用高斯來表述狀態量(位置與速度)的情況,因此對於當前時刻k機器人的狀態我們需要通過兩個變數進行定義:
1. 估計值 - 各狀態量的均值,也可以用符號u進行表示;
2. 協方差矩陣Pk;
在此處我們僅僅只使用了機器人的位置和速度來表示狀態量,但是在實際操作和運算中,狀態矩陣的定義可以包括其他任何有用的變數.
現在,我們需要了解一下當前狀態(t = k-1)和下一時刻的預測狀態(t = k)的情況.我們需要注意的是,雖然我們無法明確指出當前分佈中的哪一種情況是當前真實的狀態,但是我們仍然可以通過預測方程得到新時刻的分佈情況.因為預測方程並非只作用於某一個具體的狀態量,而是作用與當前分佈中所包含的所有情況.
在下圖中我們用矩陣Fk來表示預測步驟:
現在思考一下,我們如何運用一個矩陣來預測下一時刻機器人的位置和速度呢?在此,我們將會運用如下所示的運動學方程進行預測:
轉化為矩陣形式有:
很明顯,這是一個勻速運動模型,新時刻的位置P_new = P_previous + time*v .現在我們已經求得了預測矩陣(或稱為狀態轉移矩陣),但是我們仍然不知道如何去更新協方差矩陣.
為了更新協方差矩陣,我們需要一個新的公式即:
該公式表示,將當前分佈中所包含的所有情形(點)乘以預測矩陣就能夠得到更新或的協方差矩陣了.
以上所屬內容,可以總結為以下兩個公式:
4. 外部影響
即使計算到了現在,我們也仍未囊括所有資訊.因為在系統中存在這一些不隨狀態量變化而發生變化的變數 - 外部世界的一些因素也能對系統產生影響.
以火車執行為例,火車的操作員可能會控制執行開關使得火車加速.類似的,在此機器人的案例中,機器人導航系統可能會發出指令使得機器人驅動輪轉向或停止.因此,在計算中我們需要考慮到這類變數對系統的影響.通常來說我們將這類變數稱作為系統的控制變數,用符號進行表示.
現在我們假設對於機器人這個案例,已知其外部控制量為加速度a(由運動開關或控制命令進行控制),則之前所述的執行方程可更新為:
矩陣形式為:
其中Bk為控制矩陣,而為控制變數所組成的矩陣(n x 1).系統中的控制量只有在其已知或可獲取時才加入到方程中,並不是必不可少的.
現在在讓我們思考另外一個問題,如果我們預測並不是一個100%精準的模型,那麼將會發生什麼?
5. 外部不確定性
假如我們的變數是通過系統本身的屬性以及已知的外部影響而計算得到的,那麼狀態計算將不會有太大問題.但假如影響系統的量我們無法明確獲取到他們的值呢?
仍然以我們的機器人為例,在運動過程中其驅動輪可能打滑,機器人也有可能撞到地面上的隆起物(地面凹凸不平)使速度變慢.我們很難表示或追蹤這些因素所產生的影響.一旦這些情況發生,我們的預測結果就很有可能與實際結果產生很大的偏差,因為我們並未在數學模型中考慮到這些因素.
但是不用擔心,在數學上對於這種情況是有解決辦法的.我們在每一次的預測步驟中加入新的不確定性來表示這些世界中存在但是我們無法明確表示的變數所帶來的影響.
原始估計中的每個狀態都會通過變換方程運動到一個新的狀態範圍中.
如圖下所示,上一時刻狀態中的每一個狀態點(藍色區域表示)都會移動到一個新的區域範圍中(該範圍新加入了不確定性,用綠色圓表示).也就是說,我們將世界中無法表示且會對狀態變數帶來不確定性的影響視作噪聲(KF中通常為白噪聲,協方差用表示).
由於加入了額外的噪聲協方差,因此很明顯相較與之前推導中所產生的分佈區域,在在這種情況下的分佈區域會明顯不一樣.
現在,讓我們將新的噪聲協方差加入到之前所推匯出來的公式中:
這兩個公式代表了卡爾曼濾波器中的預測部分,是卡爾曼濾波器五個基礎公式中的前兩個.其中估計值為根據上一時刻系統狀態和當前時刻系統控制量所得到的系統估計值,該估計值又叫做先驗估計值,為各變數高斯分佈的均值.而為協方差矩陣,代表了不確定性,它是由上一時刻的協方差矩陣和外部噪聲的協方差一起計算得到的.
好了,現在我們已經得到了系統的預測(估計)值了.但是在真是的系統中,我們往往還能通過感測器得到一些能反映系統狀態的測量值.
6. 測量值(觀測值)
通常我們會在機器人上安裝一些感測器,這些感測器的返回值(測量量)能夠讓我們瞭解更多與機器人當前狀態有關的資訊.例如,我們現在有兩個感測器,一個返回位置資訊另一個返回速度資訊.這兩個感測器都能夠間接的提供一些機器人運動狀態的資訊(sensor operate on a state and produce a set of readings).
需要注意的是,感測器返回的資訊其單位和尺度可能與我們在預測步驟中所跟蹤的狀態量的單位和尺度有所不同.因此通常我們使用矩陣Hk來表示狀態量與感測器所觀測的測量量之間的關係.
通常,我們能夠計算出感測器返回值的分佈情況(通過感測器出廠說明中的引數等資訊):
需要注意的是卡爾曼濾波器之所以好用的一個原因就是它可以很好的處理感測器的噪聲.也就是說,感測器的觀測值(或測量值)是具有一定程度的不可靠性的(由噪聲引起),因此感測器的讀數是並非是一個精確值,而是一個帶有不確定區域的範圍.
通過感測器的觀測,我們可以猜測我們的機器人當前所處的狀態.但是因為不確定性的存在,因此某些狀態比其他狀態有更大的可能行被視作感測器的讀數.
在卡爾曼濾波器中我們用符號來表示感測器因為噪聲所帶來的不確定性的協方差.而感測器觀測值的均值就等於感測器讀數值,通常用來進行表示.
因此,現在我們有了兩個不同的高斯分佈:一個代表預測步驟,一個代表感測器的觀測.
我們現在需要根據預測步驟(粉色)和感測器讀數(綠色)來調整感測器讀數的分佈(此處是在解釋後文將會提到的卡爾曼增益).
那麼現在哪一種才是當前最有可能的狀態情況呢?對於某一組感測器讀數(Z1,Z2),我們現在有兩種可能:
1. 當前的感測器測量為錯誤的,不可以很好的代表機器人的狀態(當前機器人的狀態應該完全與預測步驟的分佈相同);
2. 當前的感測器測量為正確的,可以很好的代表當前機器人的狀態(當前機器人的狀態應該完全與感測器觀測分佈相同);
如果現在我們假設這兩種可能行都是正確的,我們將其分佈相乘,就能得到一個新的如下所示的分佈:
相乘後剩下的區域為高量部分,在此區域中兩種假設情況都是高量.這就意味這這個新生成的分佈區域比之前任一一種假設都要精確(完全相信預測步驟和完全相信感測器觀測),因此該區域為對機器人當前狀態的最好估計(可以用下圖表示).
以上的表述證明了,當你講兩個具有獨立的均值和協方差矩陣(seperate means and covariance matrics)的高斯分佈相乘時,就能夠得到一個具有新的均值和協方差矩陣的高斯分佈.也許你能夠發現:一定存在一個數學表示式能夠通過舊的引數值計算出來這個新的引數值.
7. 結合高斯
現在我們一起來找到這個數學公公式.為了易於理解,我們首先從一維情況開始推導.一維的高斯分佈可以表示為:
現在我們想要知道,當我們講兩個高斯曲線(分佈)相乘時會發生什麼.下圖中的藍色部分表示相乘後得到的新的高斯分佈的結果.
現在我們將一階高斯分佈的公式帶入到上式中有:
現在讓我們將上式改寫為矩陣形式的表示式,並令Σ為高斯分佈的協方差矩陣:
K被成為卡爾曼增益,我們將會在後面用到它.
8. 整合
我們有兩個分佈:預測步驟的分佈為,感測器觀測值的分佈為.我們將這兩個分佈帶入到公式:
能夠求解得到重疊區域:
因此卡爾曼增益為:
在以上的推導過程中我們可以發現每一個變數都乘上了變數,因此可以約掉,又發現在計算協方差矩陣時可以約掉.因此簡化後的公式為:
在卡爾曼濾波器中我們稱這三個公式為更新步驟,這也是卡爾曼濾波器五個基本公式中的最後三個.
此處的估計值才是當前時刻機器人最後的最優估計值,而為當前時刻機器人最有估計的協方差矩陣.這兩個變數將會當成是當前時刻最後的輸出值進入到下一次的卡爾曼濾波器計算過程中.
卡爾曼濾波器工作流程示意圖大致如下:
9. 總結
卡爾曼濾波器預算分為預測和更新兩步,切具有五個基本方程:
1. 預測步驟:
2. 更新步驟
需要注意的是,卡爾曼濾波器只適用於線性系統,在真實世界中我們所面對的系統通常為非線性系統,因此工程更常用的是擴充套件卡爾曼濾波.擴充套件卡爾曼濾波與卡爾曼濾波十分相似,唯一不同點在與擴充套件卡爾曼濾波需要對系統進行線性化(雅克比矩陣).
標籤: 基礎理論 好文要頂 關注我 收藏該文 李小銘關注 - 4
粉絲 - 15 +加關注 0 0 « 上一篇: Python學習(一) —— matplotlib繪製三維軌跡圖
» 下一篇: SLAM中的優化理論(一)—— 線性最小二乘
<div class="postDesc"><img src="/skins/gray/images/speech.gif" align="absmiddle"> posted on <span id="post-date">2017-05-14 18:48</span> <a href="https://www.cnblogs.com/leexiaoming/">李小銘</a> 閱讀(<span id="post_view_count">3748</span>) 評論(<span id="post_comment_count">1</span>) <a href="https://i.cnblogs.com/EditPosts.aspx?postid=6852483" rel="nofollow">編輯</a> <a href="#" onclick="AddToWz(6852483);return false;">收藏</a></div>
評論
<div class="feedbackItem">
<div class="feedbackListSubtitle">
<div class="feedbackManage">
<span style="color:#006699"><a href="#4101306" class="layer">#1樓</a><a name="4101306" id="comment_anchor_4101306"></a><span id="comment-maxId" style="display:none;">4101306</span><span id="comment-maxDate" style="display:none;">2018/10/29 22:11:53</span></span> <span class="comment_actions"></span>
</div>
<div style="padding-left:30px;"><img src="/skins/gray/images/arrow.gif" align="absmiddle"> <a id="a_comment_author_4101306" class="feedbackmanagelink" href="https://www.cnblogs.com/ncuneugcj/" target="_blank">sabers</a> <a href="http://msg.cnblogs.com/send/sabers" title="傳送站內短訊息" class="sendMsg2This"> </a><font style="color:gray;font-size:10px;font-weight:normal;"> | <span class="comment_date">2018-10-29 22:11</span></font>
</div>
</div>
<div class="feedbackCon">
<div id="comment_body_4101306" class="blog_comment_body">請問博主,類似這種關於 slam 的部落格,你一般在哪裡找? 或者搜尋什麼關鍵字才能找到國外的優秀文章部落格。謝謝!</div><div class="comment_vote"><a href="javascript:void(0);" class="comment_digg" onclick="return voteComment(4101306,'Digg',this)">支援(0)</a><a href="javascript:void(0);" class="comment_bury" onclick="return voteComment(4101306,'Bury',this)">反對(0)</a></div>
</div>
</div>
<div id="comments_pager_bottom"></div></div><script type="text/javascript">var commentManager = new blogCommentManager();commentManager.renderComments(0);</script>
重新整理評論
重新整理頁面
返回頂部
註冊使用者登入後才能發表評論,請
登入 或
註冊,
訪問網站首頁。
</div>
</div>
</div>
<!-- left ends -->
<!-- right starts -->
<div id="right">
<!-- 右側工具部分 -->
<div id="right_content">
公告
暱稱: 李小銘園齡: 1年9個月
粉絲: 15
關注: 4 +加關注
<div id="calendar"><div id="blog-calendar" style=""><table id="blogCalendar" class="Cal" cellspacing="0" cellpadding="0" title="Calendar">
<tbody><tr><td colspan="7"><table class="CalTitle" cellspacing="0">
<tbody><tr><td class="CalNextPrev"><a href="javascript:void(0);" onclick="loadBlogCalendar('2018/11/01');return false;"><</a></td><td align="center">2018年12月</td><td class="CalNextPrev" align="right"><a href="javascript:void(0);" onclick="loadBlogCalendar('2019/01/01');return false;">></a></td></tr>
</tbody></table></td></tr><tr><th class="CalDayHeader" align="center" abbr="日" scope="col">日</th><th class="CalDayHeader" align="center" abbr="一" scope="col">一</th><th class="CalDayHeader" align="center" abbr="二" scope="col">二</th><th class="CalDayHeader" align="center" abbr="三" scope="col">三</th><th class="CalDayHeader" align="center" abbr="四" scope="col">四</th><th class="CalDayHeader" align="center" abbr="五" scope="col">五</th><th class="CalDayHeader" align="center" abbr="六" scope="col">六</th></tr><tr><td class="CalOtherMonthDay" align="center">25</td><td class="CalOtherMonthDay" align="center">26</td><td class="CalOtherMonthDay" align="center">27</td><td class="CalOtherMonthDay" align="center">28</td><td class="CalOtherMonthDay" align="center">29</td><td class="CalOtherMonthDay" align="center">30</td><td class="CalWeekendDay" align="center">1</td></tr><tr><td class="CalWeekendDay" align="center">2</td><td align="center">3</td><td align="center">4</td><td align="center">5</td><td align="center">6</td><td align="center">7</td><td class="CalWeekendDay" align="center">8</td></tr><tr><td class="CalWeekendDay" align="center">9</td><td align="center">10</td><td align="center">11</td><td align="center">12</td><td align="center">13</td><td align="center">14</td><td class="CalWeekendDay" align="center">15</td></tr><tr><td class="CalWeekendDay" align="center">16</td><td align="center">17</td><td align="center">18</td><td align="center">19</td><td align="center">20</td><td align="center">21</td><td class="CalWeekendDay" align="center">22</td></tr><tr><td class="CalWeekendDay" align="center">23</td><td align="center">24</td><td align="center">25</td><td class="CalTodayDay" align="center">26</td><td align="center">27</td><td align="center">28</td><td class="CalWeekendDay" align="center">29</td></tr><tr><td class="CalWeekendDay" align="center">30</td><td align="center">31</td><td class="CalOtherMonthDay" align="center">1</td><td class="CalOtherMonthDay" align="center">2</td><td class="CalOtherMonthDay" align="center">3</td><td class="CalOtherMonthDay" align="center">4</td><td class="CalOtherMonthDay" align="center">5</td></tr>
<div id="leftcontentcontainer">
<div id="blog-sidecolumn"><div id="sidebar_search" class="sidebar-block">
搜尋
常用連結
最新隨筆
- 1. SLAM中的優化理論(二)- 非線性最小二乘
- 2. SLAM中的優化理論(一)—— 線性最小二乘
- 3. 卡爾曼濾波器推導與解析 - 案例與圖片
- 4. Python學習(一) —— matplotlib繪製三維軌跡圖
- 5. ZED 相機 && ORB-SLAM2安裝環境配置與ROS下的除錯
我的標籤
- C++(2)
- SLAM中優化理論(2)
- 程式設計-Python(1)
- 基礎理論(1)
- 實踐-環境配置(1)
<h1 class="catListTitle">隨筆檔案<span style="font-size:11px;font-weight:normal">(5)</span></h1>
<div>
<div class="catList">
<a id="CatList_LinkList_0_Link_0" class="listitem" href="https://www.cnblogs.com/leexiaoming/archive/2017/07.html">2017年7月 (2)</a>
</div>
<div class="catList">
<a id="CatList_LinkList_0_Link_1" class="listitem" href="https://www.cnblogs.com/leexiaoming/archive/2017/05.html">2017年5月 (1)</a>
</div>
<div class="catList">
<a id="CatList_LinkList_0_Link_2" class="listitem" href="https://www.cnblogs.com/leexiaoming/archive/2017/03.html">2017年3月 (2)</a>
</div>
</div>
<h1 class="catListTitle">文章檔案<span style="font-size:11px;font-weight:normal">(2)</span></h1>
<div>
<div class="catList">
<a id="CatList_LinkList_1_Link_0" class="listitem" href="https://www.cnblogs.com/leexiaoming/archives/2017/04.html" rel="nofollow">2017年4月 (2)</a>
</div>
</div>
積分與排名
- 積分 - 13598
- 排名 - 35458
最新評論
- 1. Re:Python學習(一) —— matplotlib繪製三維軌跡圖
- 請教博主,在繪製三維軌跡時,如果Z軸設定為時間軸,如何讀取時間格式(h:mm)形式的資料?
- 2. Re:Python學習(一) —— matplotlib繪製三維軌跡圖
- 兄弟。你的資料為啥不粘貼出來。。。。。。。》。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。......
- 3. Re:卡爾曼濾波器推導與解析 - 案例與圖片
- 請問博主,類似這種關於 slam 的部落格,你一般在哪裡找? 或者搜尋什麼關鍵字才能找到國外的優秀文章部落格。謝謝!
- 4. Re:Python學習(一) —— matplotlib繪製三維軌跡圖
- 已經解決了,灰常謝謝這麼細緻的回覆
- 5. Re:Python學習(一) —— matplotlib繪製三維軌跡圖
- @mianduo 首先讀取的資料儲存成為一個 array,當成矩陣的形式即可,比如:4x4矩陣[1,2,3,4][3,4,5,6][5,6,7,8][6,7,8,9]這個矩陣在 txt 檔案中就是按照......
閱讀排行榜
- 1. ZED 相機 && ORB-SLAM2安裝環境配置與ROS下的除錯(7949)
- 2. Python學習(一) —— matplotlib繪製三維軌跡圖(7641)
- 3. SLAM中的優化理論(二)- 非線性最小二乘(6678)
- 4. 卡爾曼濾波器推導與解析 - 案例與圖片(3749)
- 5. SLAM中的優化理論(一)—— 線性最小二乘(1406)
評論排行榜
- 1. ZED 相機 && ORB-SLAM2安裝環境配置與ROS下的除錯(6)
- 2. Python學習(一) —— matplotlib繪製三維軌跡圖(5)
- 3. 卡爾曼濾波器推導與解析 - 案例與圖片(1)
- 4. SLAM中的優化理論(二)- 非線性最小二乘(1)
推薦排行榜
</div>
<!-- //右側工具部分 -->
</div>
<!-- right ends -->
<div class="clear"></div>
Copyright ©2018 李小銘