©作者 | Doreen
01 問題描述
預先知道事物未來的狀態總是很有價值的！
√ 預知颱風的路線可以避免或減輕重大自然災害的損失。
√ 敵國打過來的導彈，如果能夠高精度預測軌跡，就能有效攔截。
√ 操控無人機，需要知道下一刻飛機的方向、速度不斷修正，才能精準控制、迴避各種風險。
這是一個狀態估計問題
如下圖所描繪的，在 k 個（一個或多個）時間點上，
基於初始的狀態資訊
一系列觀測資料
給定控制輸入
以及系統的運動和觀測模型，力求預測系統在每一時刻的真實狀態
圖 1.1: 狀態估計問題示意圖
以自動駕駛為例，圖中符號的含義如下：
● xˇ: 設計的軌跡，比如預先計算得出的理想軌跡。
● w: 駕駛過程中各種操作引入的噪聲，稱為過程噪聲。
● x: 在理想軌跡之上混入了過程噪聲的真實軌跡。
● t 下標表示時間。
● n: 觀測噪聲。
● y: 觀測資料：對真實軌跡的觀測，其中包含觀測噪聲。
每一個 t − 1 時刻，系統處於
狀態，輸入控制訊號
狀態變為
這一過程用運動模型描述
此時觀測到
這一個過程用觀測模型描述
可以看出 t 時刻狀態，在時間序列上只與 t − 1 時刻的狀態有關，即具有一階馬爾科夫性。
02 EKF 演算法
最開始人們對這一問題做了簡化，假設模型是線性的，噪聲符合高斯分佈，提出了 Kalman Filter。然而總存在某些問題不符合線性與高斯假設，人們又繼續探索。
2.1、Bayes Filter
由於系統狀態與觀測之間的因果關係，人們首先想到了貝葉斯定理。對於狀態x, 由先驗p(x), 後驗概率 p(x|y), 推到觀測物件發生的概率。
考慮到系統的馬爾科夫性， t 時刻狀態與 t − 1 時刻之前的狀態和控制 輸入都沒有關係，忽略無關項，改寫後如下。
2.2、演算法改進
上述 Bayes Filter 是精確的模型，然而實際應用卻有兩個困難：
1.概率密度函式是個無窮維函式空間，需要無先的記憶體，置信度 p(xk|0, v1:k, y0:k) 需要近似。一般通過取樣的方法解決。
2.由於積分的關係， 計算量會非常大， 一般通過將運動模型和觀測模型 線性化來解決或通過蒙特卡洛積分。
有很多的科研成果聚焦在如何解決這兩個問題， 代表性的如本文下面討論 的 EKF， Particl Filter ,Sigmapoint Kalman Filter 等。另外在 Bayes Filter 近似演算法中很重要一點是保持馬爾科夫性。
2.3、EKF
EKF 在近似 Bayes Filter 的時候採用的近似方案是將模型線性化，假設噪聲是高斯的。emmm, 捂臉，.... 說的就是因為模型非線性、噪聲非高斯。
所以回到 Bayes Filter，然後 Bayes Filter 有難以克服的障礙，需要近似解，而解決方案繞一圈竟然又回到了線性高斯假設的原點，這是騙鬼呢麼？
真的， 這就是著名的 EKF。在Kalman,1960發表了他的Kalman FIlter論文後， 他遇到了 NASA Ames Reserach Center 的 Stanley F.Schmidt，他們一起對 Kalman Filter 做了改進，最終應用在 NASA Apollo 專案的航空器軌跡估計上。看EKF曾經多麼NB閃閃。
他們的改進主要集中在 3 個方面：
i 將原有的工作擴充套件到非線性模型和非高斯噪聲領域。
ii 對當前最優解做線性化，以減輕非線性帶來的影響。
iii 將原來的濾波器改造為現在標準的預測和校正兩步。
原來的 Bayes Filter 經過線性近似和高斯假設後變為：
可以看出公式 (1.18) 與公式 (1.6) 出發點是一樣的， 只是 (1.18) 給出了可 性的均值方差求解公式。
對照公式 ((1.18)) 左右兩邊，可以得出：
狀態預測：
卡爾曼增益：
修正項
03 程式設計實現
3.1 幾個重要的矩陣
本文只側重 SLAM 中有關的應用。文末有示例程式碼連結。
首先簡單介紹 EKF 中的幾個重要矩陣。
3.2 系統狀態矩陣 X
由機器人 robot 位姿和 n 個路標 landmank 的座標組成的 (3 + 2 × n,1) 矩陣， 稱為系統狀態矩陣。
位移變數的單位可以是米或釐米，角度變數的單位是度或弧度。
3.3 協方差矩陣 P
協方差矩陣 P 在 SLAM 中十分重要，描述了機器人位姿間的相關性，機器人位姿與路標間的相關性以及路標與路標間的相關性。P 是對稱的。
P 矩陣是對稱的，E=D，F=G(代表了最後一個路標與第一個路標的協 方差項)。在增加新的路標點的時候，不僅要在對角線上增加各自的協方差項，還要增加與機器人的協方差項 (第一行、第一列)，以及與其他路標的協方差項。
矩陣內容大致如下：
協方差矩陣最初如果沒有觀測到路標點，會只包含 A，隨著新的路標點不斷加入，維度會越來越大。
考慮到初始化的不確定性，即使機器人位姿是精確的，給 A 一個合適的初值也是明智的，反之可能會在求解過程中遇到一些問題。
3.4 卡爾曼增益： K
卡爾曼增益供我們選擇的機會，選擇從觀測路標點獲取的資訊和機器人自帶的里程計資訊哪個更可靠。
K 矩陣與狀態矩陣是互相對應的，每一行代表一個狀態變數。
第 1 行代表狀態矩陣X第1行發生變化引起的增益，
其中
表示沿x軸位移引起的增益，
對應繞 x 軸旋轉引起的增益。
第 2 行代表狀態矩陣X第2行發生變化引起的增益，其中
表示沿y軸位移引起的增益，
對應繞y軸旋轉引起的增益。
. . . 以此類推。
3.5 觀測模型的 Jacobian 矩陣： H
機器人對路標點的觀測可以表示為：
路標點座標儲存在系統狀態矩陣裡, 直接讀取就可以：
觀測模型的 Jacobian 矩陣： 路標觀測向量對機器人狀態估計值
的求導。
簡化表示成如下形式：
具體的例子如：
如果觀測的路標是
那麼在
對應位置填上相反的值，
(路標沒有旋轉)
3.6 預測模型的 Jacobian 矩陣： A
預測模型：在給定機器人座標和控制下預測機器人下一時刻將要到達的座標。
其中 (x, y) 表示當前機器人座標，△t 表示驅動的增量，q 是誤差項，f 對機器人蔘數求導：
emmm …帶 q 的誤差那一被項被忽略掉了。用 △tcosθ 代替 △x,△tsinθ 代替 △y 就變成了
3.7 SLAM 中的 EKF 獨有的 Jacobian
表達由機器人位置 (x, y) 引起的對路標預測誤差誤差
對路標的預測對路標座標的 Jacobian
3.8 過程噪聲 Q 和 W
過程噪聲是與控制訊號成比例的高斯噪聲，記作 Q。如果
C 與里程計的準確性有關，準確性高對應的係數就大，通常是需要實驗調參的。
3.9 測量噪聲： R 和 V
觀測噪聲是與路標測量 (range,bearing)有關的 2×2 矩陣，
形如
其中 r 與 range 有關， c 是常數， 如果 range 誤差 1cm， c 應該取值 0.01，表示高斯噪聲方差。
如果 bearing 誤差 1 度， b=1。
常數與測量裝置的準確性有關。
有了上面幾個關鍵矩陣的鋪墊，下面終於 EKF 流程了：
3.10 使用里程計更新系統當前狀態
更新的是狀態矩陣 X(1.3.2) 前 3 個量
3.11 更新預測模型 Jacobian
3.12 更新過程噪聲
過程噪聲是與控制緊密相關的。
3.13 更新機器人位姿有關的協方差
(協方差矩陣頂部 3 × 3) 塊。
3.14 用重新觀測的路標資料更新當前狀態
預測路標
僅從機器人位置資訊更新狀態是不夠準確的，還可以通過對路 標點的重新觀測修正機器人位姿。對於機器人當前位姿 (x, y) 和上一時刻路 標座標 (λx, λy) 可以計算距離和角度 h：
還可以直接通過感測器計算路標。
3.15 更新測量誤差矩陣 R， V
其中 r = range, c = 0.01, bd = 1
3.16 計算 kalman 增益
3.17 新的狀態向量
3.18 把新的路標加入狀態
用新的路標點更新狀態向量 X
3.19 更新協方差矩陣
更新協方差矩陣 P: 對角線元素
更新協方差矩陣 P: 機器人-路標相關元素
更新協方差矩陣 P:路標-機器人相關元素
更新協方差矩陣 P:路標-路標相關元素
3.20 實驗結果
圖中藍色是理想軌跡，黑色是真實軌跡，紅色是估計的結果。
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