State Estimation and SLAM Problem
SLAM Problem
SLAM (Simultaneous localization and mapping)
Localization
以下是自走車在不同時間 (tk) 移動到不同的位置 (xk)
自走車會透過控制指令 (uk) 進行移動
當中也會有一些誤差 (wk)
所以我們可以把移動到新位置的公式寫作 xk=f(xk−1,ukwk)
下一個位置 (xk) 等於以下三者一起計算出來的
前一個位置 (xk−1)
移動的 noise (wk)
Mapping
自走車行走中，同時也會用 sensor 來感測周圍的標的物 (mj)，計算測量值 (zk,j)
在時間 k 測量目標 j 的結果可以寫成 zk,j=h(mj,xk,vk,j)
在時間 k 對目標 j 測量的值 (zk,j) 等於以下三者所計算出來的
測量的 noise (vk,j)
Problem
可以看到 localize 和 mapping 都會有誤差，分別是 wk 和 vk,j
wk 造成粉紅色的 location uncertainty
vk,j 造成藍色的 map uncertainty
而 SLAM 問題就是如何利用帶有這些 noise 的資料，也就是 u 和 z，來估計自走車狀態 (x)、和目標物狀態 (m)
Probability Graphical Model for SLAM Problem
我們可以將 SLAM problem 轉為 probability graphical model 的樣式
藍色節點為 visible node 代表可以直接被偵測
Noise (wt,vt) 被自動 model 在這個機率模型裡面
可以看到在 SLAM problem 的兩個式子都可以在圖上被表現出來
xt=f(xt−1,ut,wt)zt=h(m,xt,vt)
Front-end
SLAM 有三個前端架構，結合在一起可以在特定時間 t 建立車子的位置 (pose, xt) 和環境的地圖 (m)
若感測器為視覺感測器，那麼 front-end 又稱 visual odometry
Prediction
從前一刻的 xt−1 還有同時刻的 ut 來推測該時刻的 xt
Tracking
根據目前觀測資訊 zt 來推測目前的位置狀態 xt
Mapping
根據目前觀測資訊 zt 來建構地圖 m
Back-end
上面的觀測、預測通常會有誤差，累積後會產生錯誤，所以需要由 back-end 來修飾
在 back-end 有兩種做法來補償誤差，一種是 filter-based 一種是 graph-based
Filter-based error compensation
Graph-based error compensation
發現 error 的方法稱為 loop closure detection
確定繞一圈後，將資訊丟給 error compensation
Probability Theory and Bayes Filter
Statistical Inference
Inference 是從 premises 來推測出 consequences 的過程
因為不可能從所有 premises 來推測，所以只拿一些 samples 來測，叫做 statistical inference
Hypothesis Testing (Top-down)
透過不斷假設 parameters 並帶入 samples 來評估結果是否正確
Estimation (Bottom-up)
直接從 samples 去推測 parameters
Maximum Likelihood Estimation
最有名的 estimation 是 MLE
MLE 看看哪一個參數 (θk) 的 likelihood 分布最有可能產生 samples (x)
可以看到在不同 θ 分布下產生 x 的機率為 likelihood (p(x∣θ))
而所有不同參數 p(x∣θ) 所連成的線就是 likelihood function (f(θ))
Maximum a Posteriori Estimation
MLE 採樣時可能產生誤差，猜錯真正的 likelihood 分布，所以有了 MAP
MAP 多考量了 prior 的機率，所以可以降低 MLE 採樣錯誤所產生的誤差
Example
MLE
以投硬幣舉例，總共投了五次 (Tail, Tail, Tail, Head, Tail)
我們想知道正面機率 (θ) 是多少，才造成上面的結果 (x)，也就是要求 likelihood (p(x∣θ))
Find pmaxθ(1−θ)4dθdθ(1−θ)4=(1−θ)4+4θ(1−θ)3(−1)=(1−θ)3(5θ−1)=0θ=0.2
MAP
p(x)p(θ)p(x∣θ)=p(θ∣x)
同樣的題目，我們定義 prior 為 Discrete Uniform Distribution，也就是各為 1/11 (0.0 到 1.0)
p(x)=∑θp(x,θ)=∑θp(x∣θ)p(θ)1/111/111/11⋮×(0)1(1)4(0.1)1(0.9)4(0.2)1(0.8)4⋮=0.0000.2130.333⋮ 因為 prior 是 uniform distribution，所以其實 MAP 沒有改變太多
Bayesian Approach
不斷用 prior + likelihood 來計算 posterior 並更新假設
不再是看誰讓 likelihood 或 posterior 最大化，而是直接觀察 parameters 的分布
Bayes Filter
Bayesian approach 會將參數 (θ) 帶入 prior、likelihood 計算出 posterior。posterior 再重新做為下一輪的 belief 進行計算，最終找出參數 (θ) 的分布
State Prediction
我們也可以應用於 SLAM 的 probability graphical model
P(xt∣z1:t−1,u1:t)=∫P(xt∣xt−1,z1:t−1,u1:t)P(xt−1∣z1:t−1,u1:t)dxt−1=∫P(xt∣xt−1,ut)P(xt−1∣z1:t−1,u1:t)dxt−1
我們可以用 u1 到 ut
以及 z1 到 zt
來估算當前時間點的 xt
化簡中，因為 xt−1 為已知
所以 xt−1 之前的觀測資訊就都不重要了
我們可以將式子簡寫成以下樣子 (bel 代表 belief)
bel(xt)=∫P(xt∣xt−1,ut)bel(xt)dxt−1 bel(xt) 為當下時刻的估測
當下時刻還沒觀測到同時間的 zt
bel(xt) 為前一時刻的估測
已經觀測到該時間點的 tt−1
Measurement Update
當我們得到了觀測資訊 (zt) 就可以用來更新位置資訊 (xt)
P(xt∣z1:t,u1:t)=P(zt∣z1:t−1,u1:t)P(zt∣xt,z1:t−1,u1:t)P(xt∣z1:t−1,u1:t)=ηP(zt∣xt,z1:t−1,u1:t)P(xt∣z1:t−1,u1:t)=ηP(zt∣xt)P(xt∣z1:t−1,u1:t)
可以看到左邊的算式中有 z1:t
但右邊的只有 z1:t−1
一樣可以轉成以下的 belief 表達方式
bel(xt)=ηP(zt∣xt)bel(xt) bel 表示有 zt 資訊
bel 表示沒有 zt 資訊
Bayes Filter Algorithm
Bayes filter 就是上面兩個式子遞迴互相更新所產生
寫成 pseudo code 表示為
Example
一開始的 bel(xt) 是 prior distribution
感測到 z1 得到 likelihood (P(zt∣xt)) 後可以更新 bel(x1)
接著有了 u2 可以更新運動狀態 bel(x2)
接著一樣感測到 z2 更新回去得到最新的 bel(x2)
Kalman Filter
機器人透過 prior 分布 (原本就預測的目的地, xkpre) 和 likelihood 分布 (感應地標後得到的目的地, xkobs)
結合兩個分布就能得到 posterior 分布 (最終決定移動的點, xkest)
Kalman Filter
以下是 kalman filter 對狀態的建模
Noise 的分布 (Q, R) 在假設中皆為高斯分布 (Gaussian distribution)
Goal: 用可觀察的東西，對不可觀察的東西做出預測
假設所有變換都是線性的，可以得到以下公式:
xk=Axk−1+Buk+wkzk=Hxk+vk Kalman Filter Computation Steps
共有四個參數: A,B,Q,R
xkpre=Axk−1est+Buk
Pkpre=APk−1estAT+Q
Kk=PkpreHT(HPkpreHT+R)−1
xkest=xkpre+Kk(zk−Hxkpre)
Pkest=(I−KkH)Pkpre Extended Kalman Filter
Kalman filter 線性以及高斯分布的假設讓所有運算都變得簡單
但在現實中的狀況大多不是這麼簡單
於是在 Kalman filter 上加入"線性近似"的概念，就得到了 extended Kalman filter
也就是在預測狀態的 mean 時，改用 1st order Taylor expansion 來求
可以看到藍色線就是求得的近似線性值
我們可以得到新的公式:
Prediction Model \& Observation Model
xk=f(xk−1,uk)+wkzk=h(xk)+vk Fk=∂x∂f(x^k−1,uk),Hk=∂x∂h(x^k) xk=f(x^k−1,uk)+Fk(xk−1−x^k−1)+wkzk=h(x^k)+Hk(xk−x^k)+vk Extended Kalman Filter Computation Steps
在原本的 Kalman filter 時，計算中的 A, H 都是固定的
而在 EKF 中，每個時間點都須根據前一刻的估計值，重新用第一階的 taylor expansion 求得線性近似
xkpre=f(xk−1est,uk)Pkpre=FkPk−1preFkT+QKk=PkpreHT(HPkpreHT+R)−1xkest=xkpre+Kk(zk−Hxkpre)Pkest=(I−KkH)Pkpre
EKF-SLAM
為了將 EKF 應用到 SLAM 問題中，首先要將 pose, landmark 定義成狀態 (state)
sk=robot’s posex,y,θ,landmark 1m1,x,m1,y,landmark 2m2,x,m2,y,…,landmark nmn,x,mn,yT 而狀態的分布如下，其中 covariance 可以拆成四個部分 (pose 本身關聯、pose 和 map 關聯、map 本身關聯)
xyθm1,xm1,y⋮mn,xmn,y→μ=[xm],Σ=[ΣxxΣmxΣxmΣmm]
Prediction Model
x′y′θ′=xyθ+−ωvsin(θ)+ωvsin(θ+ωtΔt)ωvcos(θ)−ωvcos(θ+ωtΔt)ωΔt
Linearized the velocity motion model (對 prediction model 微分)
Ftx=∂(x,y,θ)T∂fx′y′θ′=100010−ωtvtcos(θ)+ωtvtcos(θ+ωtΔt)−ωtvtsin(θ)+ωtvtsin(θ+ωtΔt)1 Observation Model
zi=[qatan2(δx,δy)−θ],δ=[mi,x−xmi,y−y],q=δTδ
Linearized the observation model (對 observation model 微分)
Hi=∂(x,y,θ,mi,x,mi,y)∂zi=q1[−qδxδy−qδy−δx0−qqδx−δyqδyδx]
矩陣大小為 2 * 5
5 為自走車 pose 的 3 個變數 + 地標的 2 個座標 (x, y)
Summary
我們可以建構關聯性的轉換矩陣，把上面兩種的局部結果，轉換到全局的整個狀態情況下
Prediction model (3*n, n 為狀態數量)
Ft=100010001000⋯⋯⋯000T×Ftx
Observation model (5*n, 左上角 3 個與 pose 有關, 其餘每 2 個為 landmark)
Ht=10000010000010000000⋯⋯⋯⋯⋯00000000100000100000⋯⋯⋯⋯⋯00000×Hti
有了全局的 Ft,Ht 就可以帶進 EKF 中開始計算
xkpre=f(xk−1est,uk)Pkpre=FkPk−1preFkT+QKk=PkpreHT(HPkpreHT+R)−1xkest=xkpre+Kk(zk−Hxkpre)Pkest=(I−KkH)Pkpre
