# SLAM Back-end (I)

## State Estimation and SLAM Problem

### SLAM Problem

* SLAM (Simultaneous localization and mapping)
  * 希望能達到同時定位自己並且建構整個地圖

#### Localization

以下是自走車在不同時間 ($$\mathbf{t}\_k$$) 移動到不同的位置 ($$\mathbf{x}\_k$$)

* 自走車會透過控制指令 ($$\mathbf{u}\_k$$) 進行移動
* 當中也會有一些誤差 ($$\mathbf{w}\_k$$)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRz51syLmZa0yYc6%2Fslam_x.png?generation=1585719143454505\&alt=media)

所以我們可以把移動到新位置的公式寫作 $$\mathbf{x}*k = f(\mathbf{x}*{k-1}, \mathbf{u}\_k \mathbf{w}\_k)$$

* 下一個位置 ($$\mathbf{x}\_k$$) 等於以下三者一起計算出來的
  * 前一個位置 ($$\mathbf{x}\_{k-1}$$)
  * 控制指令 ($$\mathbf{u}\_k$$)
  * 移動的 noise ($$\mathbf{w}\_k$$)

#### Mapping

自走車行走中，同時也會用 sensor 來感測周圍的標的物 ($$\mathbf{m}*j$$)，計算測量值 ($$\mathbf{z}*{k, j}$$)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRz8PimvtHcTHCRg%2Fslam_z.png?generation=1585719137463076\&alt=media)

在時間 k 測量目標 j 的結果可以寫成 $$\mathbf{z}\_{k, j} = h(\mathbf{m}\_j, \mathbf{x}*k, \mathbf{v}*{k, j})$$

* 在時間 k 對目標 j 測量的值 ($$\mathbf{z}\_{k, j}$$) 等於以下三者所計算出來的
  * 目標物 ($$\mathbf{m}\_j$$)
  * 車子位置 ($$\mathbf{x}\_k$$)
  * 測量的 noise ($$\mathbf{v}\_{k, j}$$)

#### Problem

可以看到 localize 和 mapping 都會有誤差，分別是 $$\mathbf{w}*k$$ 和 $$\mathbf{v}*{k, j}$$

* $$\mathbf{w}\_k$$ 造成粉紅色的 location uncertainty
* $$\mathbf{v}\_{k, j}$$ 造成藍色的 map uncertainty

而 SLAM 問題就是如何利用帶有這些 noise 的資料，也就是 $$\mathbf{u}$$ 和 $$\mathbf{z}$$，來估計自走車狀態 ($$\mathbf{x}$$)、和目標物狀態 ($$\mathbf{m}$$)

### Probability Graphical Model for SLAM Problem

我們可以將 SLAM problem 轉為 probability graphical model 的樣式

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzC5OnfbZ0oBpUE%2Fslam_probability_graphical_model.png?generation=1585719144437576\&alt=media)

* 每個變數變成了一個節點
* 藍色節點為 visible node 代表可以直接被偵測
* 箭頭代表了因果的關係
* Noise ($$\mathbf{w}\_t, \mathbf{v}\_t$$) 被自動 model 在這個機率模型裡面

可以看到在 SLAM problem 的兩個式子都可以在圖上被表現出來

$$\begin{aligned} &\mathbf{x}*{t}=f\left(\mathbf{x}*{t-1}, \mathbf{u}*{t}, \mathbf{w}*{t}\right)\ &\mathbf{z}*{t}=h\left(\mathbf{m}, \mathbf{x}*{t}, \mathbf{v}\_{\mathbf{t}}\right) \end{aligned}$$

#### Front-end

SLAM 有三個前端架構，結合在一起可以在特定時間 t 建立車子的位置 (pose, $$\mathbf{x}\_t$$) 和環境的地圖 ($$\mathbf{m}$$)

若感測器為視覺感測器，那麼 front-end 又稱 visual odometry

* Prediction
  * 從前一刻的 $$\mathbf{x}\_{t-1}$$ 還有同時刻的 $$\mathbf{u}\_t$$ 來推測該時刻的 $$\mathbf{x}\_t$$

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzFpXWkDVxGvv-y%2Fslam_prediction.png?generation=1585719153387872\&alt=media)

* Tracking
  * 根據目前觀測資訊 $$\mathbf{z}\_t$$ 來推測目前的位置狀態 $$\mathbf{x}\_t$$

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzHrUupZqeyyCFE%2Fslam_tracking.png?generation=1585719139384971\&alt=media)

* Mapping
  * 根據目前觀測資訊 $$\mathbf{z}\_t$$ 來建構地圖 $$\mathbf{m}$$

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzJTtFqnic4O_BY%2Fslam_mapping.png?generation=1585719148345540\&alt=media)

#### Back-end

上面的觀測、預測通常會有誤差，累積後會產生錯誤，所以需要由 back-end 來修飾

在 back-end 有兩種做法來補償誤差，一種是 filter-based 一種是 graph-based

* Filter-based error compensation
  * 直接根據最近的誤差來調整
  * 即時 (online) 但整體優化不佳

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzLTfcm8jrbdtVG%2Fslam_filter_based.png?generation=1585719146494578\&alt=media)

* Graph-based error compensation
  * 每個時間點，都使用過去多個時間點的誤差來調整

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzNM8TP8D7zcyhq%2Fslam_graph_based.png?generation=1585719140458648\&alt=media)

發現 error 的方法稱為 loop closure detection

* 檢查自走車是否到達以前到過的位置
* 確定繞一圈後，將資訊丟給 error compensation

## Probability Theory and Bayes Filter

### Statistical Inference

Inference 是從 premises 來推測出 consequences 的過程

因為不可能從所有 premises 來推測，所以只拿一些 samples 來測，叫做 statistical inference

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzPjRmX04FBcbYj%2Fstatistical_inference.png?generation=1585719142370730\&alt=media)

* Hypothesis Testing (Top-down)
  * 是機器學習常用作法
  * 透過不斷假設 parameters 並帶入 samples 來評估結果是否正確
  * 最終得到最棒的 parameters
* Estimation (Bottom-up)
  * 直接從 samples 去推測 parameters

#### Maximum Likelihood Estimation

最有名的 estimation 是 MLE

MLE 看看哪一個參數 ($$\theta\_k$$) 的 likelihood 分布最有可能產生 samples ($$x$$)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzROQCmmkYm4WUp%2Fmaximum_likelihood_estimation.png?generation=1585719138547510\&alt=media)

可以看到在不同 $$\theta$$ 分布下產生 $$x$$ 的機率為 likelihood ($$p(x\mid\theta)$$)

而所有不同參數 $$p(x\mid\theta)$$ 所連成的線就是 likelihood function ($$f(\theta)$$)

#### Maximum a Posteriori Estimation

MLE 採樣時可能產生誤差，猜錯真正的 likelihood 分布，所以有了 MAP

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzTaaKydi99USXM%2Fmaximum_posteriori_estimation.png?generation=1585719141575812\&alt=media)

MAP 多考量了 prior 的機率，所以可以降低 MLE 採樣錯誤所產生的誤差

#### Example

**MLE**

以投硬幣舉例，總共投了五次 (Tail, Tail, Tail, Head, Tail)

我們想知道正面機率 ($$\theta$$) 是多少，才造成上面的結果 ($$x$$)，也就是要求 likelihood ($$p(x\mid\theta)$$)

$$\begin{aligned} &\text{Find } \max \_{p} \theta(1-\theta)^{4}\\\ &\frac{d \theta(1-\theta)^{4}}{d \theta}=(1-\theta)^{4}+4 \theta(1-\theta)^{3}(-1)=(1-\theta)^{3}(5 \theta-1)=0\\\ &\theta = 0.2 \end{aligned}$$

**MAP**

$$\frac{p(\theta) p(x | \theta)}{p(x)}=p(\theta | x)$$

同樣的題目，我們定義 prior 為 Discrete Uniform Distribution，也就是各為 1/11 (0.0 到 1.0)

$$
\frac{\begin{bmatrix}
1/11 \ 1/11 \ 1/11 \\\vdots
\end{bmatrix}
\times \begin{bmatrix}
(0)^1(1)^4 \ (0.1)^1(0.9)^4 \ (0.2)^1(0.8)^4 \\\vdots
\end{bmatrix}
}{p(x) = \sum\_\theta p(x, \theta) = \sum\_\theta p(x\mid\theta)p(\theta)}
\= \begin{bmatrix}
0.000\0.213\0.333\\\vdots
\end{bmatrix}
$$

因為 prior 是 uniform distribution，所以其實 MAP 沒有改變太多

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzVnqh9Zyf4Sd0z%2Fmle_map_comparison.png?generation=1585719147927116\&alt=media)

### Bayesian Approach

不斷用 prior + likelihood 來計算 posterior 並更新假設

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzXJ1IKsb58MSAH%2Fbayesian_approach.png?generation=1585719152682494\&alt=media)

不再是看誰讓 likelihood 或 posterior 最大化，而是直接觀察 parameters 的分布

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzZPGw8z3TE3Bhd%2Fbayesian_approach_example.png?generation=1585719145479297\&alt=media)

### Bayes Filter

Bayesian approach 會將參數 ($$\theta$$) 帶入 prior、likelihood 計算出 posterior。posterior 再重新做為下一輪的 belief 進行計算，最終找出參數 ($$\theta$$) 的分布

#### State Prediction

我們也可以應用於 SLAM 的 probability graphical model

$$\begin{aligned} P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) &=\int P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{x}*{t-1}, \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) \mathrm{P}\left(\mathbf{x}*{t-1} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) d x\_{t-1} \ &=\int P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{x}*{t-1}, \mathbf{u}*{t}\right) \mathrm{P}\left(\mathbf{x}*{t-1} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) d x\_{t-1} \end{aligned}$$

* 我們可以用 $$\mathbf{u}\_1$$ 到 $$\mathbf{u}\_t$$
* 以及 $$\mathbf{z}\_1$$ 到 $$\mathbf{z}\_t$$
* 來估算當前時間點的 $$\mathbf{x}\_t$$

> - 化簡中，因為 $$\mathbf{x}\_{t-1}$$ 為已知
> - 所以 $$\mathbf{x}\_{t-1}$$ 之前的觀測資訊就都不重要了

我們可以將式子簡寫成以下樣子 (bel 代表 belief)

$$
\overline{b e l}\left(\mathbf{x}*{\mathrm{t}}\right)=\int P\left(\mathbf{x}*{\mathrm{t}}
\mid \mathbf{x}*{t-1}, \mathbf{u}*{t}\right) b e l\left(\mathbf{x}*{\mathrm{t}}\right) d x*{t-1}
$$

* $$\overline{b e l}\left(\mathbf{x}\_{\mathrm{t}}\right)$$ 為當下時刻的估測
  * 當下時刻還沒觀測到同時間的 $$\mathbf{z}\_t$$
* $$b e l\left(\mathbf{x}\_{\mathrm{t}}\right)$$ 為前一時刻的估測
  * 已經觀測到該時間點的 $$\mathbf{t}\_{t-1}$$

#### Measurement Update

當我們得到了觀測資訊 ($$\mathbf{z}\_t$$) 就可以用來更新位置資訊 ($$\mathbf{x}\_t$$)

$$\begin{aligned} P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{z}*{1: t}, \mathbf{u}*{1: t}\right) &=\frac{P\left(\mathbf{z}*{\mathrm{t}} | \mathbf{x}*{\mathrm{t}}, \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right)}{P\left(\mathbf{z}*{\mathrm{t}} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right)} \ &=\eta P\left(\mathbf{z}*{\mathrm{t}} | \mathbf{x}*{\mathrm{t}}, \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{z}*{1: t-1}, \mathbf{u}*{1: t}\right) \ &=\eta P\left(\mathbf{z}*{\mathrm{t}} | \mathbf{x}*{\mathrm{t}}\right) P\left(\mathbf{x}*{\mathrm{t}} | \mathbf{z}*{1: t-1}, \mathbf{u}\_{1: t}\right) \ \end{aligned}$$

* 可以看到左邊的算式中有 $$\mathbf{z}\_{1:t}$$
* 但右邊的只有 $$\mathbf{z}\_{1:t-1}$$

一樣可以轉成以下的 belief 表達方式

$$
b e l\left(\mathbf{x}*{\mathrm{t}}\right)=\eta P\left(\mathbf{z}*{\mathrm{t}} | \mathbf{x}*{t}\right) \overline{b e l}\left(\mathbf{x}*{\mathrm{t}}\right)
$$

* $$bel$$ 表示有 $$\mathbf{z}\_t$$ 資訊
* $$\overline{bel}$$ 表示沒有 $$\mathbf{z}\_t$$ 資訊

#### Bayes Filter Algorithm

Bayes filter 就是上面兩個式子遞迴互相更新所產生

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzato178yabgjKz%2Fbayes_filter_algorithm.png?generation=1585719151698150\&alt=media)

寫成 pseudo code 表示為

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzcmU04rv4m5gHL%2Fbayes_filter_algorithm2.png?generation=1585719150490054\&alt=media)

**Example**

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3oDOyr6mCxc8gXYFHg%2F-M3oDRzeltL7NVI8A-9u%2Fbayes_filter_algorithm3.png?generation=1585719149713981\&alt=media)

* 一開始的 $$bel(\mathbf{x}\_t)$$ 是 prior distribution
* 感測到 $$\mathbf{z}\_1$$ 得到 likelihood ($$P(\mathbf{z}\_t\mid\mathbf{x}\_t)$$) 後可以更新 $$bel(\mathbf{x}\_1)$$
* 接著有了 $$\mathbf{u}\_2$$ 可以更新運動狀態 $$\overline{bel}(\mathbf{x}\_2)$$
* 接著一樣感測到 $$\mathbf{z}\_2$$ 更新回去得到最新的 $$bel(\mathbf{x}\_2)$$

## Kalman Filter

機器人透過 prior 分布 (原本就預測的目的地, $$x\_k^{\text{pre}}$$) 和 likelihood 分布 (感應地標後得到的目的地, $$x\_k^{\text{obs}}$$)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U80d0QjZFFcX2lD%2Frobot_prior_likelihood.jpg?generation=1585902390053099\&alt=media)

結合兩個分布就能得到 posterior 分布 (最終決定移動的點, $$x\_k^{\text{est}}$$)

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U83Q3OjK3nvpsfX%2Frobot_posterior.jpg?generation=1585902390982308\&alt=media)

### Kalman Filter

以下是 kalman filter 對狀態的建模

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U85Fa9dGAeynivJ%2Fkalman_filter.jpg?generation=1585902388006715\&alt=media)

* x-axis: 時間方向
* y-axis: 可觀察、不可觀察
* Noise 的分布 (Q, R) 在假設中皆為高斯分布 (Gaussian distribution)
* Goal: 用可觀察的東西，對不可觀察的東西做出預測

假設所有變換都是線性的，可以得到以下公式:

$$
\begin{aligned}
\&x\_{k}=A x\_{k-1}+B u\_{k}+w\_{k}\\
\&z\_{k}=H x\_{k}+v\_{k}
\end{aligned}
$$

#### Kalman Filter Computation Steps

共有四個參數: $$A, B, Q, R$$

1. 預測下一個狀態

$$
x\_{k}^{p r e}=A x\_{k-1}^{e s t}+B u\_{k}
$$

1. 計算預測的 covariance

$$
P\_{k}^{p r e}=A P\_{k-1}^{e s t} A^{T}+Q
$$

1. 計算 Kalman-gain

$$
K\_{k}=P\_{k}^{p r e} H^{T}\left(H P\_{k}^{p r e} H^{T}+R\right)^{-1}
$$

1. 預測狀態的 mean

$$
x\_{k}^{e s t}=x\_{k}^{p r e}+K\_{k}\left(z\_{k}-H x\_{k}^{p r e}\right)
$$

1. 預測狀態的 covariance

$$
P\_{k}^{e s t}=\left(I-K\_{k} H\right) P\_{k}^{p r e}
$$

### Extended Kalman Filter

Kalman filter 線性以及高斯分布的假設讓所有運算都變得簡單

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U87nNPMW2Aqhay-%2Fkalman_filter_distribution.jpg?generation=1585902384915155\&alt=media)

但在現實中的狀況大多不是這麼簡單

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U89gQf5LerPAs2n%2Fkalman_filter_distribution2.jpg?generation=1585902386904532\&alt=media)

於是在 Kalman filter 上加入"線性近似"的概念，就得到了 extended Kalman filter

也就是在預測狀態的 mean 時，改用 1st order Taylor expansion 來求

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U8CUFBrQH9tGzjt%2Fextended_kalman_filter_taylor.jpg?generation=1585902388955116\&alt=media)

可以看到藍色線就是求得的近似線性值

我們可以得到新的公式:

* Prediction Model \\& Observation Model

$$
\begin{aligned}
\&x\_{k}=f\left(x\_{k-1}, u\_{k}\right)+w\_{k}\\
\&z\_{k}=h\left(x\_{k}\right)+v\_{k}
\end{aligned}
$$

* Jacobian Matrix:

$$
F\_{k}=\frac{\partial f\left(\hat{x}*{k-1}, u*{k}\right)}{\partial x}, H\_{k}=\frac{\partial h\left(\hat{x}\_{k}\right)}{\partial x}
$$

* Linearized System

$$
\begin{aligned}
\&x\_{k}=f\left(\hat{x}*{k-1}, u*{k}\right)+F\_{k}\left(x\_{k-1}-\hat{x}*{k-1}\right)+w*{k}\\
\&z\_{k}=h\left(\hat{x}*{k}\right)+H*{k}\left(x\_{k}-\hat{x}*{k}\right)+v*{k}
\end{aligned}
$$

#### Extended Kalman Filter Computation Steps

在原本的 Kalman filter 時，計算中的 A, H 都是固定的

而在 EKF 中，每個時間點都須根據前一刻的估計值，重新用第一階的 taylor expansion 求得線性近似

$$\begin{aligned} \&x\_{k}^{p r e}=f\left(x\_{k-1}^{e s t}, u\_{k}\right)\ \&P\_{k}^{\text {pre}}=F\_{k} P\_{k-1}^{\text {pre}} F\_{k}^{T}+Q\ \&K\_{k}=P\_{k}^{p r e} H^{T}\left(H P\_{k}^{p r e} H^{T}+R\right)^{-1}\ \&x\_{k}^{e s t}=x\_{k}^{p r e}+K\_{k}\left(z\_{k}-H x\_{k}^{p r e}\right)\ \&P\_{k}^{e s t}=\left(I-K\_{k} H\right) P\_{k}^{p r e} \end{aligned}$$

### EKF-SLAM

為了將 EKF 應用到 SLAM 問題中，首先要將 pose, landmark 定義成狀態 (state)

$$
s\_{k}=\left(\underbrace{x, y, \theta}*{\text{robot's pose}}, \underbrace{m*{1, x}, m\_{1, y}}*{\text{landmark 1}}, \underbrace{m*{2, x}, m\_{2, y}}*{\text{landmark 2}}, \ldots, \underbrace{m*{n, x}, m\_{n, y}}\_{\text{landmark n}}\right)^{T}
$$

而狀態的分布如下，其中 covariance 可以拆成四個部分 (pose 本身關聯、pose 和 map 關聯、map 本身關聯)

$$\left\[\begin{array}{c} x \ y \ \theta \ m\_{1,x} \ m\_{1,y} \ \vdots \ m\_{n,x} \ m\_{n,y} \end{array}\right] \rightarrow \mu=\left\[\begin{array}{c} \mathbf{x} \ \mathbf{m} \end{array}\right], \Sigma=\left\[\begin{array}{cc} \Sigma\_{\mathbf{x x}} & \Sigma\_{\mathbf{x m}} \ \Sigma\_{\mathbf{m x}} & \Sigma\_{\mathbf{m m}} \end{array}\right]$$

#### Prediction Model

* Prediction Model

$$\left\[\begin{array}{l} x^{\prime} \ y^{\prime} \ \theta^{\prime} \end{array}\right]=\left\[\begin{array}{l} x \ y \ \theta \end{array}\right]+\left\[\begin{array}{c} -\frac{v}{\omega} \sin (\theta)+\frac{v}{\omega} \sin \left(\theta+\omega\_{t} \Delta t\right) \ \frac{v}{\omega} \cos (\theta)-\frac{v}{\omega} \cos \left(\theta+\omega\_{t} \Delta t\right) \ \omega \Delta t \end{array}\right]$$

* Linearized the velocity motion model (對 prediction model 微分)

$$
F\_t^x = \frac{\partial f}{\partial(x,y,\theta)^T}\left\[\begin{array}{l}
x^{\prime} \\
y^{\prime} \\
\theta^{\prime}
\end{array}\right] =
\left\[\begin{array}{ccc}
1 & 0 & -\frac{v\_{t}}{\omega\_{t}} \cos (\theta)+\frac{v\_{t}}{\omega\_{t}} \cos \left(\theta+\omega\_{t} \Delta t\right) \\
0 & 1 & -\frac{v\_{t}}{\omega\_{t}} \sin (\theta)+\frac{v\_{t}}{\omega\_{t}} \sin \left(\theta+\omega\_{t} \Delta t\right) \\
0 & 0 & 1
\end{array}\right]
$$

#### Observation Model

![](https://2991100231-files.gitbook.io/~/files/v0/b/gitbook-legacy-files/o/assets%2F-LhyC30yNfTP1YdCIj83%2F-M3z8RGz0vf9Gcyqsmi1%2F-M3z8U8GI-hrPEhUdxWL%2Fekf_slam_observation.jpg?generation=1585902386129409\&alt=media)

* Given observation model

$$z\_{i}=\left\[\begin{array}{c} \sqrt{q} \ \operatorname{atan} 2\left(\delta\_{x}, \delta\_{y}\right)-\theta \end{array}\right], \delta=\left\[\begin{array}{c} m\_{i, x}-x \ m\_{i, y}-y \end{array}\right], q=\delta^{T} \delta$$

* Linearized the observation model (對 observation model 微分)

$$H^{i}=\frac{\partial z\_{i}}{\partial\left(x, y, \theta, m\_{i, x}, m\_{i, y}\right)}=\frac{1}{q}\left\[\begin{array}{ccccc} -\sqrt{q} \delta\_{x} & -\sqrt{q} \delta\_{y} & 0 & \sqrt{q} \delta\_{x} & \sqrt{q} \delta\_{y} \ \delta\_{y} & -\delta\_{x} & -q & -\delta\_{y} & \delta\_{x} \end{array}\right]$$

* 矩陣大小為 2 \* 5
  * 2 為某個地標的 x, y 座標
  * 5 為自走車 pose 的 3 個變數 + 地標的 2 個座標 (x, y)
* 如果今天觀察 l 個地標
  * 那矩陣大小就是 (2l) \* (3 + 2l)

#### Summary

我們可以建構關聯性的轉換矩陣，把上面兩種的局部結果，轉換到全局的整個狀態情況下

* Prediction model (3\*n, n 為狀態數量)

$$F\_{t}=\left\[\begin{array}{llllll} 1 & 0 & 0 & 0 & \cdots & 0 \ 0 & 1 & 0 & 0 & \cdots & 0 \ 0 & 0 & 1 & 0 & \cdots & 0 \end{array}\right]^{T} \times F\_{t}^{x}$$

* Observation model (5\*n, 左上角 3 個與 pose 有關, 其餘每 2 個為 landmark)

$$H\_{t}=\left\[\begin{array}{ccccccccc} 1 & 0 & 0 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0 \ 0 & 1 & 0 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0 \ 0 & 0 & 1 & 0 & \cdots & 0 & 0 & 0 & 0 & \cdots & 0 \ 0 & 0 & 0 & 0 & \cdots & 0 & 1 & 0 & 0 & \cdots & 0 \ 0 & 0 & 0 & 0 & \cdots & 0 & 0 & 1 & 0 & \cdots & 0 \end{array}\right] \times H\_{t}^{i}$$

有了全局的 $$F\_{t}, H\_{t}$$ 就可以帶進 EKF 中開始計算

$$\begin{aligned} \&x\_{k}^{p r e}=f\left(x\_{k-1}^{e s t}, u\_{k}\right)\ \&P\_{k}^{\text {pre}}={\color{red}F\_{k}} P\_{k-1}^{\text {pre}} {\color{red}F\_{k}^{T}}+Q\ \&K\_{k}=P\_{k}^{p r e} {\color{red}H^{T}}\left({\color{red}H} P\_{k}^{p r e} {\color{red}H^{T}}+R\right)^{-1}\ \&x\_{k}^{e s t}=x\_{k}^{p r e}+K\_{k}\left(z\_{k}-{\color{red}H} x\_{k}^{p r e}\right)\ \&P\_{k}^{e s t}=\left(I-K\_{k} {\color{red}H}\right) P\_{k}^{p r e} \end{aligned}$$
