?作者 |?Doreen

01 問題描述

預先知道事物未來的狀態(tài)總是很有價值的垂蜗！

√ 預知臺風的路線可以避免或減輕重大自然災害的損失耗绿。

√ 敵國打過來的導彈，如果能夠高精度預測軌跡这敬，就能有效攔截展辞。

√ 操控無人機，需要知道下一刻飛機的方向划提、速度不斷修正枫弟，才能精準控制、回避各種風險鹏往。

這是一個狀態(tài)估計問題

圖 1.1: 狀態(tài)估計問題示意圖

以自動駕駛為例淡诗，圖中符號的含義如下：

● xˇ: 設(shè)計的軌跡，比如預先計算得出的理想軌跡。

● w: 駕駛過程中各種操作引入的噪聲韩容，稱為過程噪聲款违。

● x: 在理想軌跡之上混入了過程噪聲的真實軌跡。

● t 下標表示時間群凶。

● n: 觀測噪聲插爹。

● y: 觀測數(shù)據(jù)：對真實軌跡的觀測，其中包含觀測噪聲座掘。

02 EKF 算法

最開始人們對這一問題做了簡化递惋，假設(shè)模型是線性的，噪聲符合高斯分布溢陪，提出了 Kalman Filter萍虽。然而總存在某些問題不符合線性與高斯假設(shè)，人們又繼續(xù)探索形真。

2.1杉编、Bayes Filter

由于系統(tǒng)狀態(tài)與觀測之間的因果關(guān)系，人們首先想到了貝葉斯定理咆霜。對于狀態(tài)x, 由先驗p(x), 后驗概率 p(x|y), 推到觀測對象發(fā)生的概率邓馒。

考慮到系統(tǒng)的馬爾科夫性， t 時刻狀態(tài)與 t ? 1 時刻之前的狀態(tài)和控制 輸入都沒有關(guān)系蛾坯，忽略無關(guān)項光酣，改寫后如下。

2.2脉课、算法改進

上述 Bayes Filter 是精確的模型救军，然而實際應用卻有兩個困難：

1.概率密度函數(shù)是個無窮維函數(shù)空間，需要無先的內(nèi)存倘零，置信度 p(xk|0, v1:k, y0:k) 需要近似唱遭。一般通過采樣的方法解決。

2.由于積分的關(guān)系呈驶， 計算量會非常大拷泽， 一般通過將運動模型和觀測模型 線性化來解決或通過蒙特卡洛積分。

有很多的科研成果聚焦在如何解決這兩個問題袖瞻， 代表性的如本文下面討論 的 EKF司致， Particl Filter ,Sigmapoint Kalman Filter 等。另外在 Bayes Filter 近似算法中很重要一點是保持馬爾科夫性聋迎。

2.3脂矫、EKF

EKF 在近似 Bayes Filter 的時候采用的近似方案是將模型線性化，假設(shè)噪聲是高斯的砌庄。emmm, 捂臉羹唠，.... 說的就是因為模型非線性、噪聲非高斯娄昆。

所以回到 Bayes Filter佩微，然后 Bayes Filter 有難以克服的障礙，需要近似解萌焰，而解決方案繞一圈竟然又回到了線性高斯假設(shè)的原點哺眯，這是騙鬼呢么？

真的扒俯， 這就是著名的 EKF奶卓。在Kalman,1960發(fā)表了他的Kalman FIlter論文后， 他遇到了 NASA Ames Reserach Center 的 Stanley F.Schmidt撼玄，他們一起對 Kalman Filter 做了改進夺姑，最終應用在 NASA Apollo 項目的航空器軌跡估計上≌泼停看EKF曾經(jīng)多么NB閃閃盏浙。

他們的改進主要集中在 3 個方面：

i 將原有的工作擴展到非線性模型和非高斯噪聲領(lǐng)域。

ii 對當前最優(yōu)解做線性化荔茬，以減輕非線性帶來的影響废膘。

iii 將原來的濾波器改造為現(xiàn)在標準的預測和校正兩步。

原來的 Bayes Filter 經(jīng)過線性近似和高斯假設(shè)后變?yōu)椋?/p>

可以看出公式?(1.18)?與公式?(1.6)?出發(fā)點是一樣的慕蔚，??只是?(1.18)?給出了可?性的均值方差求解公式丐黄。

對照公式?((1.18))?左右兩邊，可以得出：

狀態(tài)預測：

卡爾曼增益：

修正項

03 編程實現(xiàn)

3.1 幾個重要的矩陣

本文只側(cè)重 SLAM 中有關(guān)的應用孔飒。文末有示例代碼鏈接灌闺。

首先簡單介紹 EKF 中的幾個重要矩陣。

3.2?系統(tǒng)狀態(tài)矩陣?X

由機器人 robot 位姿和 n 個路標 landmank 的坐標組成的?(3 + 2 × n,1)?矩陣十偶，??稱為系統(tǒng)狀態(tài)矩陣菩鲜。

位移變量的單位可以是米或厘米，角度變量的單位是度或弧度惦积。

3.3?協(xié)方差矩陣?P

協(xié)方差矩陣 P 在 SLAM 中十分重要接校，描述了機器人位姿間的相關(guān)性，機器人位姿與路標間的相關(guān)性以及路標與路標間的相關(guān)性狮崩。P 是對稱的蛛勉。

P 矩陣是對稱的，E=D睦柴，F(xiàn)=G(代表了最后一個路標與第一個路標的協(xié)?方差項)诽凌。在增加新的路標點的時候，不僅要在對角線上增加各自的協(xié)方差項坦敌，還要增加與機器人的協(xié)方差項?(第一行侣诵、第一列)痢法，以及與其他路標的協(xié)方差項。

矩陣內(nèi)容大致如下：

協(xié)方差矩陣最初如果沒有觀測到路標點杜顺，會只包含 A财搁，隨著新的路標點不斷加入，維度會越來越大躬络。

考慮到初始化的不確定性尖奔，即使機器人位姿是精確的，給 A 一個合適的初值也是明智的穷当，反之可能會在求解過程中遇到一些問題提茁。

3.4?卡爾曼增益：? K

卡爾曼增益供我們選擇的機會，選擇從觀測路標點獲取的信息和機器人自帶的里程計信息哪個更可靠馁菜。

K 矩陣與狀態(tài)矩陣是互相對應的茴扁，每一行代表一個狀態(tài)變量。

第?1?行代表狀態(tài)矩陣X第1行發(fā)生變化引起的增益汪疮，

其中

表示沿x軸位移引起的增益丹弱，

對應繞 x 軸旋轉(zhuǎn)引起的增益。

第?2?行代表狀態(tài)矩陣X第2行發(fā)生變化引起的增益铲咨，?

其中

表示沿y軸位移引起的增益躲胳，

對應繞y軸旋轉(zhuǎn)引起的增益。

? . . . ?以此類推纤勒。

3.5 觀測模型的 Jacobian 矩陣：? H

機器人對路標點的觀測可以表示為：

路標點坐標保存在系統(tǒng)狀態(tài)矩陣里, 直接讀取就可以：

觀測模型的 Jacobian 矩陣：?路標觀測向量對機器人狀態(tài)

估計值

的求導坯苹。

簡化表示成如下形式：

具體的例子如：

如果觀測的路標是

,?

那么在

對應位置填上相反的值，

(路標沒有旋轉(zhuǎn))

3.6 預測模型的 Jacobian 矩陣：? A

預測模型：在給定機器人坐標和控制下預測機器人下一時刻將要到達的坐標摇天。

其中?(x, y)?表示當前機器人坐標粹湃，△t 表示驅(qū)動的增量，q 是誤差項泉坐，f 對機器人參數(shù)求導：

emmm …帶 q 的誤差那一被項被忽略掉了为鳄。用?△tcosθ?代替?△x,△tsinθ?代替?△y 就變成了

3.7?SLAM?中的?EKF?獨有的?Jacobian

表達由機器人位置?(x,?y)?引起的對路標預測誤差誤差

對路標的預測對路標坐標的?Jacobian

3.8?過程噪聲?Q?和?W

過程噪聲是與控制信號成比例的高斯噪聲，記作 Q腕让。如果

C 與里程計的準確性有關(guān)孤钦，準確性高對應的系數(shù)就大，通常是需要實驗調(diào)參的纯丸。

3.9?測量噪聲：? R 和 V

觀測噪聲是與路標測量?(range,bearing)有關(guān)的?2×2?矩陣偏形，

形如

。?

其中 r 與 range 有關(guān)觉鼻，? c 是常數(shù)俊扭，??如果 range 誤差 1cm， c 應該取值?0.01坠陈，表示高斯噪聲方差萨惑。

如果 bearing 誤差 1 度捐康，? b=1。

常數(shù)與測量設(shè)備的準確性有關(guān)庸蔼。

有了上面幾個關(guān)鍵矩陣的鋪墊吹由，下面終于 EKF 流程了：

3.10?使用里程計更新系統(tǒng)當前狀態(tài)

更新的是狀態(tài)矩陣?X(1.3.2)?前?3?個量

3.11?更新預測模型?Jacobian

3.12?更新過程噪聲

過程噪聲是與控制緊密相關(guān)的。

3.13?更新機器人位姿有關(guān)的協(xié)方差

(協(xié)方差矩陣頂部 3 × 3)?塊朱嘴。

3.14?用重新觀測的路標數(shù)據(jù)更新當前狀態(tài)

預測路標

僅從機器人位置信息更新狀態(tài)是不夠準確的，還可以通過對路?標點的重新觀測修正機器人位姿粗合。對于機器人當前位姿?(x, y)?和上一時刻路?標坐標?(λx, λy)?可以計算距離和角度 h：

還可以直接通過傳感器計算路標萍嬉。

3.15?更新測量誤差矩陣?R，?V

隙疚。其中 r = range, c =?0.01, bd = 1

3.16?計算?kalman?增益

3.17?新的狀態(tài)向量

3.18?把新的路標加入狀態(tài)

用新的路標點更新狀態(tài)向量?X

3.19?更新協(xié)方差矩陣

更新協(xié)方差矩陣?P:?對角線元素

更新協(xié)方差矩陣?P:?機器人-路標相關(guān)元素

更新協(xié)方差矩陣?P:路標-機器人相關(guān)元素

更新協(xié)方差矩陣?P:路標-路標相關(guān)元素

3.20?實驗結(jié)果

圖中藍色是理想軌跡壤追，黑色是真實軌跡，紅色是估計的結(jié)果供屉。

04 源代碼地址

h t t p s : / / g i t h u b . com/Hou?a l e x / p u b l i c S r c / b l o b /main/ ekf_slam . py

私信我領(lǐng)取目標檢測與R-CNN/數(shù)據(jù)分析的應用/電商數(shù)據(jù)分析/數(shù)據(jù)分析在醫(yī)療領(lǐng)域的應用/NLP學員項目展示/中文NLP的介紹與實際應用/NLP系列直播課/NLP前沿模型訓練營等干貨學習資源行冰。