# FAST-LIO2: Fast Direct LiDAR-inertial Odometry

- 出版年份：2021
- 作者：Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, Fu Zhang
- 發表期刊：IEEE
- 是否提供源代碼：是

## Abstracts

現今研究技術中，在 unknown environment 中建立 3D map 的議題越來越普遍，而在當前的方法中， Vision-based SLAM 雖然在定位功能上具有極高準確率，但卻對影像移動所產生的 illumination variation / motion blur 干擾極大，導致它只能得到一個較為稀疏的 feature map；而另外一種主流技術，real-time dense mapping，其準確性主要是建立在一些視覺性的感測器的測量結果，則會受到影像畫質高低與硬體運算能力的限制。

文中介紹了 FAST-LIO2：一種快速、穩健且通用的 LiDAR-IMU 框架，它具有以下兩個關鍵的特性：

- *ikd-Tree data structure*

設計一種建立於 incremental k-d tree 資料結構，稱為 *ikd-Tree*，用這種結構來進行 point cloud map 的建立。

**(使用 ikd-Tree 的好處與功能：相鄰點的搜尋 + 點雲的更新 + 動態重新平衡)**

- *direct method*

直接將每個點雲加入 global map 中，而不提取相關 feature，並在隨後進行 global map updating 的修正，也就是 mapping 的動作。

由於 ikd-Tree 的計算效率極高，作者利用這種結構直接將原始點加入 global map，使系統在一些劇烈運動或是非常混段的環境中，也具有相當高的定位準確性，**這個特性改善了傳統 Visual-SLAM 缺點**。

結合上述 ikd-Tree 的結構與 direct method 的方法到一個具有緊密耦合特性的 LiDAR-IMU 系統中 (也就是 FAST-LIO)，為了加快該系統的計算速度，另外設計一個新的數學等效公式，用以重新計算 Kalman gain，這個公式的應用可成功降低計算複雜度，這樣的新系統稱作 FAST-LIO2。

*FAST-LIO 系統的主要運作模式：*

*The system uses an IMU to compensate each point's motion via a rigorous back-propagation step and estimates the system's full state via an on-manifold iterated Kalman filter.*

## Method

為了執行狀態估計，<font color = "red">FAST-LIO2 繼承 FAST-LIO 的 tightly-coupled iterated Kalman Filter，即 LiDAR 新掃描到的點雲會通過 **tightly-coupled iterated Kalman Filter** 框架 (紅色虛線區)</font>，加入到 global map 中，而 <font color = "blue">global map 中的地圖點則由以 incremental k-d Tree 為基礎建立而成的 **ikd-Tree** 來進行 (藍色虛線區)</font>，若當前 LiDAR 的 FoV 範圍超過了 map 邊界，系統會將該點從 ikd-Tree 中刪除。

整個系統主要流程如下圖所示：


在點雲加入 global map 的過程中， ikd-Tree 會追蹤一個大範圍立方體區域 (map size) 中的所有 map point，並用以計算狀態估計模組中的殘差，而優化後的位姿最後會將 LiDAR 新掃描到的下一組點雲，根據 odometry 的頻率來插入 ikd-Tree 中，以這樣的方式再繼續將這組點雲合併到 global map 中。

## Architecture

### State Estimation

FAST-LIO2 的 state estimation module 是從 FAST-LIO 中繼承的，並進一步融合 online calibration of LiDAR-IMU extrinsic parameters，具體步驟說明如下。

#### I. Propagation (Forward Propagation)

已知上一個 LiDAR scan 中得到的最優狀態估計為 $\bar {\rm x}_{k-1}$，且其共變異數矩陣為 $\bar{\rm P}_{k-1}$，則系統的最開始會進行 forward propagation (前向傳播)，利用 **state transition model** 中計算出的狀態轉移公式 ${\rm x}_{i + 1} = {\rm x}_i \boxplus (\Delta t {\rm f}({\rm x}_i, {\rm u}_i, {\rm w}_i))$，可以得到當前第 $k$ 個 scan 中的 state vector 與其共變異數矩陣 (設定 ${\rm w}_i = 0$)：
$$
\begin{aligned}
& \hat{\rm x}_{i + 1} = \hat{\rm x}_i \boxplus (\Delta t {\rm f}(\hat{\rm x}_i, {\rm u}_i, {\rm w}_i)) \\
& \hat{\rm P}_{i + 1} = {\rm F}_{\tilde {\rm x}_i} \hat{\rm P}_i {\rm F}^T_{\tilde{\rm x}_i} + {\rm F}_{{\rm w}_i} {\rm Q}_i {\rm F}_{{\rm w}_i}^T
\end{aligned}
$$
其中相關參數意義如下：

- ${\rm Q}_i$；表示噪聲 ${\rm w}_i$ 的變異數
- $\hat{\rm x}_0 = \bar{\rm x}_{k-1}$ 且 $\hat{\rm P}_0 = \bar{\rm P}_{k-1}$

Forward Propagation 的動作會不斷進行，直到抵達當前 ($k$-th) scan 掃描結束的瞬間為止，此時的 state vector 為 $\hat{\rm x}_k$，其共變異數矩陣為 $\hat{\rm P}_k$。

#### II. Backward Propagation and Motion Compensation

為了解決 LiDAR 在劇烈運動下產生的 motion distortion problem，作者提出 backward propagation 來解決這個問題，以下將解釋 motion distortion problem 與 backward propagation method。

- motion distortion problem (in-scan motion problem)

這個問題形成主要原因有兩個：

* LiDAR 本身產生運動
* 點雲為 LiDAR 在掃描過程中蒐集到的數據點集合

如下圖所示，假設原本實際情況 $p1$、$p2$、$p3$ 三個點是在同一直線上，LiDAR 掃描下來會因為它自身在每個時間點的姿態不同，在不同姿態底下得到的數據點就會是在不同座標系上，最後掃描結束後，蒐集到的數據點就會和真實情況有偏差(會得到"這三個點座標處於不同座標系"的資訊)。


當這些數據點形成點雲後，最後要投影到 2D 平面上，這樣就會導致和真實情況有偏差；而從直觀上來看，蒐集到的點雲數據會產生一定的變形，這就是 backward propagation method 希望能解的 in-scan motion problem (又稱作 motion distortion problem)。

*實際測試*

| With Backward Propagation | Without Backward Propagation |
| -------- | -------- |
| | |

由上方可以看出，這個 motion distortion problem 主要影響的部分除了路徑閉合問題外，路徑計算的過程中也會受到數據變形的影響。

- backward propagation method

由於IMU input 會隨著時間產生漂移誤差，所以必須對應同一時間點 LiDAR 產生的點雲，用已校正 IMU input 的漂移誤差。

但在 LiDAR 蒐集到的 feature point 上面會產生剛剛提到 motion distortion 的問題，所以必須先計算出正確的 LiDAR point，而在計算出正確的 IMU 資訊(在 state vector 裡面)後，才可以結合 LiDAR 的 feature point 進行投影，形成點雲。


在這個方法中，作者利用後向傳播的方式，以 $t_k$ 時刻的位姿，推算出 $\rho_j$ 時刻的位姿，並且把 $\rho_j$ 時刻的特徵點轉換到當前 $t_k$ 時刻。

透過上述計算，得到 $\rho_j$ 時刻到 $t_k$ 時刻的相對位姿為 $^{I_k}{\check T_{I_j}} = (^{I_k}{\check R_{I_j}}, ^{I_k}{\check p_{I_j}})$

根據上述結果，我們可以通過下方公式，將 local 座標系的點測量值 $^{L_j}p_{f_j}$，轉換為掃描終點時刻的 $^{L_k}p_{f_j}$。
$$
^{L_k}p_{f_j} = ^I{\rm T}^{-1}_L {^{I_k}\check{{\rm T}}_{I_j}} {^I{{\rm T}_L}} {^{L_j}{\rm p}_{f_j}}
$$
其中，$^IT_L$ 為 LiDAR 與 IMU 之間的外參，$^{L_k}p_{f_k}$ 則表示投影到掃描終點時刻 $t_k$ 的座標，主要用於接下來的 residual computation。

#### III. Residual Computation

根據剛剛 motion compensation 內，backward propagation 的公式 $^{L_k}{\rm p}_{f_j} = ^I{\rm T}^{-1}_L {^{I_k}\hat{\rm T}_{I_j}} {^I{\rm T}_L} {^{L_j}{\rm p}_{f_j}}$，我們可以將一個 scan 中的 feature points $\lbrace ^{L_k}{\rm p}_{f_j} \rbrace$ 視為在同一個時間點 $t_k$ 得到的點，以此來重建地圖並計算殘差。

假設當前由 forward propagation 公式 $\hat{\rm x}_{i + 1} = \hat{\rm x}_i ⊞ (\Delta t {\rm f}(\hat{\rm x}_i, {\rm u}_i, {\rm w}_i))$ 預測出的 state vector 為 $\hat{\rm x}^{\cal K}_k$，則當 $\cal K = 0$ 時，代表著第一次 iteration 前，此時 $\hat{\rm x}^{\cal K}_k=\hat{\rm x}_k$。

利用 backward propagation 計算出當前的 state vector 後，我們將每個測量到的 LiDAR point $^L{\rm p}_j$ 投影到 global frame 中，得到以下結果：
$$
^G\hat{\rm p}_j^{\cal K} = {^G\hat{\rm T}^{\cal K}_{I_k}} {^I\hat{\rm T}^{\cal K}_{L_k}} {^L{\rm p}_j}
$$
其中，$^G\hat{\rm p}_j^{\cal K}$ 為我們想要求的 $t_k$ 時間點到 global 座標系下的位姿變換。

在這個位姿變換後的特徵點應該要落在它對應的特徵線/面上，但由於存在 LiDAR 的測量誤差或是 forward propagation 的推算誤差，這會導致系統推算後的特徵點無法完全落在它對應的特徵線/面上。
$$
{\rm z}^{\cal K}_j = {\rm G}_j (^G{\hat{\rm p}^{\cal K}_{f_j}} - {^G{\rm q}_j})
$$
在這個式子中，$G_j$ 為法向量 ${\rm u}_j^T$ (平面特徵) 或是反對稱矩陣 $\lfloor u_j \rfloor$ (邊緣特徵)，也就是用來計算點到面或點到線之間的距離。而在這之中，作者只考慮在 idk-Tree 所表示的地圖上搜索距離 $^L{\rm p}_j$ 最近的 5 個點，其餘點會被視為 noise 或新的觀測點。

而若是我們將 LiDAR noise 去除，假設測量出來的點都是真實的座標：
$$
^{L_j}{\rm p}^{gt}_{f_j} = ^{L_j}{\rm p}_{f_j} - ^{L_j}{\rm n}_{f_j}
$$

則當我們將這個 ground truth 代入上述座標轉換公式中轉換出的 global 座標系誤差公式，可以得到以下結果，其中殘差值 ${\rm z}^{\cal K}_j = 0$：
$$
\begin{aligned}
{\rm z}_j &= {\rm G}_j (^G{\hat{\rm p}_{f_j}} - {^G{\rm q}_j}) \\
&= {\rm G}_j ({^G\hat{\rm T}_{I_k}} {^I\hat{\rm T}_{L_k}} \color{red}{^{L_k}{\rm p}_{f_j}} - {^G{\rm q}_j}) \\
&= {\rm G}_j ({^G\hat{\rm T}_{I_k}} {^I\hat{\rm T}_{L_k}} \color{red}{(^I{\rm T}^{-1}_{L_k} {^{I_k}\check{{\rm T}}_{I_j}} {^I{{\rm T}_{L_k}}} {^{L_j}{\rm p}_{f_j}})} - {^G{\rm q}_j}) \\
&= {\rm G}_j ({^G\hat{\rm T}_{I_k}} {^{I_k}\check{{\rm T}}_{I_j}} {^I{{\rm T}_{L_k}}} \color{red}{^{L_j}{\rm p}_{f_j}} - {^G{\rm q}_j}) \\
&= {\rm G}_j ({^G\hat{\rm T}_{I_k}} {^{I_k}{\check {\rm T}}_{I_j}} {^I\hat{\rm T}_{L_k}} \color{red}{({^{L_j}{\rm p}_{f_j}} - ^{L_j}{\rm n}_{f_j})} - {^G{\rm q}_j})
\end{aligned}
$$

> **觀測模型的建立**
>
> 根據上方的計算結果，我們同時比較在狀態向量 $x$ 的內容 ${\rm x} = [\begin{matrix}{^G{\rm R}^T_I} & {^G{\rm p}^T_I} & {^G{\rm v}^T_I} & {{\rm b}^T_\omega} & {{\rm b}^T_a} & {^G{\rm g}^T} & {^I{\rm R}^T_L} & {^I{\rm p}^T_L}\end{matrix}]^T$ 中，發現 $^GT_{I_k} = (^G{\rm R}_{I_k}, ^G{\rm p}_{I_k})$ [IMU 座標系到 globale 座標系的轉換矩陣] 以及 $^I{\rm T}_L = (^I{\rm R}_L, ^I{\rm p}_L)$ [LiDAR 坐標系到 IMU 座標系] 兩個矩陣都在狀態向量 $x$ 中，因此我們可以直把上方的計算結果簡寫為 ${\rm h}_j = ({\rm x}_k, ^L{\rm p}_j)$ 的一個模型，稱作觀測模型 measurement model。

理想情況下(沒有噪聲與干擾)，${\rm h}_j ({\rm x}_k, ^L{\rm p}_j) = 0$，此時殘差為 0，模型預測(觀測)的位置和實際的位置應該是完全一樣的。

然而，實際情況下，LiDAR 會受各種情況影響而導致測量值中有噪聲的存在，因此重新加入測量噪聲，計算 $^L{\rm p}^{gt}_j = ^L{\rm p}_j - ^L{\rm p}_j$，推導出 $^L{\rm p} = ^L{\rm p}^{gt}_j + ^L{\rm p}_j$，並重新針對 ${\rm x}^{\cal K}_k = 0$ 處進行一階泰勒展開 (first order approximation)。

> **一階泰勒展開 (First Order Approximation)**
>
> 一階泰勒展開的目的主要在於要計算一個非線性函數 $h(x)$ 的近似方程，基本上是在計算該函數於展開點 $\hat{x}$ 處的值，加上從這點到 $x$ 之間的一階導數乘以 $x$ 與 $\hat{x}$ 之間的差異，具體作法如下：
>
> 1. 選擇展開點
> 我們會選擇 $\hat{x} 作為展開點，代表我們想要進行近似的基點
>
> 2. 計算函數 $h(\hat{x})$：該函數在基點的值
> 表示當系統狀態為 $\hat{x}$ 時的 measurement model 輸出
>
> 3. 計算 Jacobian Matrix
> 計算 $h(x)$ 對 $x = \hat{x}$ 的 Jacobian Matrix $H$，其中 Jacobian Matrix 中包含函數 $h$ 對每個狀態變量的偏導數，是函數於 $\hat{x}$ 點中沿各個方向的斜率，數學上以下列公式表示
> $H = \frac{\partial h}{\partial x}|_{x = \hat{x}}$
>
> 4. 計算狀態差異
> 計算狀態差異 $\tilde{x} = x - \hat{x}$，表示實際狀態與估計狀態之間的差異
>
> 5. 進行一階泰勒展開
> 將函數 $h(x)$ 在 $\hat{x}$ 附近使用一階泰勒展開來近似，可以寫為
> $$
> h(x) \approx h(\hat{x}) + H \tilde{x}
> $$
> 其中，$h(\hat{x})$ 表示在 $\hat{x}$ 的測量值，而 $H\tilde{x}$ 表示 $\hat{x}$ 到 $x$ 的線性近似誤差。
>
>
> 總結來說，以一階泰勒展開來進行近似方程計算的方式可以**將非線性模型線性化**，使我們可以用線性的 Kalman Filter 來處理。

為了使我們在更新狀態向量可以透過縣性工具去解，我們針對 $x^{\cal K}_k = 0$ 處，使用一階泰勒展開將原先的非線性觀測模型線性化，最終計算觀測模型如下
$$
\begin{aligned}
0 &= {\rm h}_j ({\rm x}_k, ^L{\rm p}_j + ^L{\rm n}_j) \simeq {\rm h}_j (\hat{\rm x}^{\cal K}_k, 0) + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k + {\rm v}_j \\
&= {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k + {\rm v}_j
\end{aligned}
$$

其中相關參數意義如下：

- ${\rm x}_k = \hat{\rm x}^{\cal K}_k ⊞ \tilde{\rm x}^{\cal K}_k$：代表 state vector
- ${\rm H}^{\cal K}_j$：代表 ${\rm h}_j ({\rm x}_k, ^{L_j}{\rm n}_{f_j})$ 相對於 $\tilde{\rm x}^{\cal K}_k$ 在 0 處取值的 Jacobian Matrix
- ${\rm v}_j$：該數值是由原始的測量噪聲 $^L{\rm n}_j$ 產生而成

由上方公式，最終可推得殘差值計算如下：
$$
{\rm z}^{\cal K}_j = {\rm h}_j (\hat{\rm x}^{\cal K}_k, 0) = {\rm u}^T_j ({^G\hat{\rm T}^{\cal K}_{I_k}} {^I\hat{\rm T}^{\cal K}_{L_k}} {^L{\rm p}_j} - ^G{\rm q}_j)
$$
其中 $u^T_j$ 等同 ${\rm G}_j$，表示最近平面的單位法向量(用來確定最近平面的方向)。
(此時我們可以稱 $z_j^{\cal K}$ 為觀測值)

### IV. Iterated Update (state update)

根據 forward propagation 計算出的狀態轉換方式 ${\rm x}_{i + 1} = {\rm x}_i ⊞ (\Delta t {\rm f}({\rm x}_i, {\rm u}_i, {\rm w}_i))$ 與 透過 backward propagation 產生的 Residual Computation 計算結果 $0 = {\rm h}_j ({\rm x}_k, ^L{\rm p}_j + ^L{\rm n}_j)$，作者使用 iterated Kalman Filter，直接在 manifold 上面進行狀態 $\rm x$ 的更新，這種做法可以避免任何 re-normalization 技術的需求。

因此，接下來要進行的是利用 Kalman Filter 來做狀態估計，所謂的狀態估計問題是指基於初始狀態訊息、一系列的觀測量、一系列的輸入量、以及系統的運動模型與觀測模型，可以計算系統在某個時刻的真實狀態估計值。

1. 利用 IMU 與 LiDAR 的輸入，首先推導出系統模型，該模型由狀態轉移模型 (State Transition Model) 與測量模型 (Measurement Model) 組成。

* State Transition Model (狀態轉移模型)
在 IMU 的 input frame 中，取第一個 IMU frame (記為 $I$) 為 global frame (記為 $G$)，並定義 $^I{\rm T}_L = (^I{\rm R}_L, ^I{\rm p}_L)$ 為 LiDAR 和 IMU 之間的外參，則狀態轉移模型建立如下：

$$
\begin{aligned}
& ^G{\rm R}_I = ^G{\rm R}_I \lfloor \omega_m-{\rm b}_\omega-{\rm n}_\omega \rfloor_\wedge \\
& ^G{\rm p}_I = ^G{\rm v}_I \\
& ^G{\rm v}_I = ^G{\rm R}_I({\rm a}_m-{\rm b_a} - {\rm n_a})+^G{\rm g}
\end{aligned}
$$
其中參數意義如下：
- $^G{\rm R}_I$、$^G{\rm p}_I$ ：表示 IMU 在 global frame 中的姿態與位置，這兩個數值為連續性狀態轉移模型的主體
- $^G{\rm g}$：代表 global frame 中的重力向量
- ${\rm a}_m$、$\omega_m$：代表 IMU 測量值
- ${\rm n_a}$、${\rm n}_\omega$：代表 ${\rm a}_m$ 與 $\omega_m$ 的測量噪聲
- ${\rm b_a}$、${\rm b}_\omega$：代表行走其間噪聲 ${\rm n_a}$、${\rm n}_\omega$ 所造成的 IMU 偏移量
- $\lfloor \omega_m-{\rm b}_\omega-{\rm n}_\omega \rfloor_\wedge$：表示針對三維向量 $\omega_m-{\rm b}_\omega-{\rm n}_\omega$ 所構成的反對稱矩陣

根據 $\boxplus$ 的定義，連續時間下的動態模型可以依據 IMU 的採樣週期 $\Delta t$ 進行離散化，最終得到以下狀態向量：
$$
{\rm x}_{i + 1} = {\rm x}_i \boxplus (\Delta t {\rm f}({\rm x}_i, {\rm u}_i, {\rm w}_i))
$$
其中參數意義如下：

- ${\rm x} \triangleq \begin{bmatrix} {^G{\rm R}^T_I} & {^G{\rm p}^T_I} & {^G{\rm v}^T_I} & {{\rm b}^T_\omega} & {\rm b}^T_{\rm a} & {^G{\rm g}^T} & {^I{\rm p}^T_L} \end{bmatrix}^T$：表示 state vector，其中 $i$ 表示 IMU 測量的索引。
- ${\rm u} \triangleq \begin{bmatrix} {\omega^T_m} & {{\rm a}^T_m} \end{bmatrix}^T$：表示輸入向量 (IMU 測量值)
- ${\rm w} \triangleq \begin{bmatrix} {{\rm n}^T_\omega} & {{\rm n}^T_{\rm a}} & {{\rm n}^T_{\rm b \omega}} & {{\rm n}^T_{\rm ba}} \end{bmatrix}^T$：表示噪音
- ${\rm f(x, u, w)} = \begin{bmatrix} {\omega_m-{\rm b}_\omega-{\rm n}_\omega} \\ {^G{\rm v}_1 + \frac{1}{2}(^G{\rm R}_I({\rm a}_m - {\rm b_a} - {\rm n_a} ) + ^G{\rm g}) \Delta t} \\ ^G{\rm R}_I ({\rm a}_m - {\rm b_a} - {\rm n_a}) + ^G{\rm g} \\ {\rm n_b \omega} \\ {\rm n_ba} \\ {0_{3 \times 1}} \\ {0_{3 \times 1}} \\ {0_{3 \times 1}} \end{bmatrix} \in \mathbb{R}^24$

* Measurement Model (觀測模型)

由於 LiDAR 在經歷連續運動時，所得到的點通常是在不同姿勢下進行採集，也代表著不同的姿式。為了修正這種 in-scan motion 的問題，作者提出 back propagation 技術，用以**估計在當下 scan 的開始瞬間每個 point 的位姿，相對於該 scan 的結束瞬間這些 point 的位姿**，透過 IMU 測量而得的數據進行建模，最後估計相對姿勢。這個做法讓我們能夠基於 LiDAR 掃描過程中的每個單獨點精確採樣時間來將這些所有的點投影到 global frame 中，用以實現校正工作。

取第 k 個 LiDAR scan，則在該 scan 掃描結束的當下，可以獲得第 k 個 scan 的 local 座標點為 $\lbrace ^L{\rm p}_j, j = 1, ..., m \rbrace$，則**因為 LiDAR 在測量時存在各種 LiDAR measurement noise，因此每個點 ${\rm p}_j$ 都會受到測距和方向所組合而成的噪聲 $^L{\rm n}_j$ 所影響**，所以真正的 LiDAR point (稱為 true point) 與測量值之間具有以下關係：
$$
^L{\rm p}^{\rm gt}_j = ^L{\rm p}_j + ^L{\rm n}_j
$$
針對這個 true point $^L{\rm p}^{gt}_j$，以 State Transition Model 中得到的相對應 LiDAR 位姿值 $^G{\rm T}_{I_k} = (^G{\rm R}_{I_k}, ^G{\rm p}_{I_k})$ 與 LiDAR 和 IMU 之間的外參 $^I{\rm T}_L = (^I{\rm R}_L, ^I{\rm p}_L)$，進行座標轉換後，可將該點準確地映射於地圖中的 local plane 上面，並滿足下列公式：
$$
0 = ^G{\rm u}^T_j (^G{\rm T}_{I_k} ^I{\rm T}_L (^L{\rm p}_j + ^L{\rm n}_j) - ^G{\rm q}_j)
$$
其中參數定義如下：
- $^G{\rm u}_j$：表示相對 local plane 的法向量
- $^G{\rm q}_j$：表示平面上的點
<img src="https://hackmd.io/_uploads/HkXm6JTn6.png" />

如上圖所示，紅色點代表 LiDAR scan 中的點 $^L{\rm p}_j$，利用這個點找到它的 true point $^L{\rm p}^{gt}_j$ 後，將該點映射於 corresponding local plane 上，可以得到最後的深藍色點 $^G{\rm q}_j$，其中紅色虛線指向 ($^G{\rm u}_j$) 的則是該點相對於 local plane 的法向量。

而由於 $^I{\rm T}_L = (^I{\rm R}_L, ^I{\rm p}_L)$、$^G{\rm T}_{I_k} = (^G{\rm R}_{I_k}, ^G{\rm p}_{I_k})$ 均包含在狀態向量 ${\rm x}_k$ 中，因此該測量方程又可以寫為：
$$
0 = {\rm h}_j ({\rm x}_k, ^L{\rm p}_j + ^L{\rm n}_j)
$$

2. 狀態的 Prior Gaussian Distribution

在 Kalman Filter 中，先驗高斯分布的意思在於我們在獲取到新的測量數據之前，會先對系統狀態做一個概率分布，而這個分布是基於之前的所有訊息，包含初始狀態估計和所有過去的測量，因此這個分布是「先驗」的，因為它是在觀察到新數據前就形成的。

在卡爾曼濾波的開始，我們需要初始化狀態估計 $\hat{\rm{x}}_0 = \hat{\rm{x}}_{k - 1}$ 以及它的共變異數矩陣 $P_0 = P_{k - 1}$，這部分的值會是前面 forward propagation 的計算結果。

除此之外，我們定義誤差狀態為真實狀態 $\rm{x}_k$ 與估計狀態 $\hat{\rm{x}}_k$ 之間的差異為 $\rm{x}_k \boxminus \hat{\rm{x}}_k$。

把上述誤差狀態的計算公式進一步展開為 $\rm{x}_k \boxminus \hat{\rm{x}}_k = \hat{\rm{x}}^{\cal K}_k \boxminus \hat{\rm{x}}_k + {\rm J}^{\cal K} \tilde{\rm x}^{\cal K}_k \sim {\cal N}(0, \hat{\rm P}_k)$，其中，${\rm J}^{\cal K}$ 表示誤差狀態的 Jacobian Matrix，用來描述如果誤差狀態有小變化的時候，估計狀態將如何變化。

${\rm J}^{\cal K} = \begin{bmatrix} {{\rm A}(\delta ^G\theta_{I_k})^{-T}} & {0_{3 \times 15}} & {0_{3 \times 3}} & {0_{3 \times 3}} \\ {0_{15 \times 3}} & {{\rm I}_{15 \times 15}} & {0_{3 \times 3}} & {0_{3 \times 3}} \\ {0_{3 \times 3}} & {0_{3 \times 15}} & {{\rm A}(\delta ^I\theta_{L_k})^{-T}} & {0_{3 \times 3}} \\ {0_{3 \times 3}} & {0_{3 \times 15}} & {0_{3 \times 3}} & {{\rm I}_{3 \times 3}} \end{bmatrix}$ 為 $(\hat{\rm x}^{\cal K}_k \boxplus \tilde{\rm x}^{\cal K}_k) \boxminus \hat{\rm x}_k$ 對 $\tilde{\rm x}^{\cal K}_k = 0$ 的偏微分

在第一次的 iteration 中，由於 $\hat{\rm x}^{\cal K}_k = \hat{\rm x}_k$，因此可得知 ${\rm J}^{\cal K} = {\rm I}$。

最後，由於共變異數矩陣 $\hat{P}_k$ 是用以量化狀態估計的不確定性的矩陣，因此我們會以這個不確定性作為高斯常態分佈的模型，表示為 $\sim {\cal N}(0, \hat{\rm P}_k)$。其中，之所以設定平均值為 0 的原因在於我們假設在最初狀態估計的時候沒有誤差，因此可以將誤差狀態的初始分布視為平均值為 0。而隨著時間推移與系統演化，誤差會根據過程的噪聲和觀測噪聲累積起來，而 $\hat{P}_k$ 則會根據這些噪聲的共變異數矩陣來更新，反映了我們對誤差分布的最新理解。

3. Maximum a-Posteriori Estimate 最大化後驗估計

根據我們在殘差估計階段中得到的 $0 = {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k + {\rm v}_j$ 可以調整為 $-{\rm v}_j = {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k \sim {\cal N} (0, {\rm R}_j)$，其中觀測噪聲 $-v_j$ 也可以假設為高斯分布，稱為觀測概率分布，其均值為 0，共變異數矩陣為 ${\rm R}_j$。

再結合剛剛計算出的先驗概率分布 $\rm{x}_k \boxminus \hat{\rm{x}}_k = \hat{\rm{x}}^{\cal K}_k \boxminus \hat{\rm{x}}_k + {\rm J}^{\cal K} \tilde{\rm x}^{\cal K}_k \sim {\cal N}(0, \hat{\rm P}_k)$ 後，可以得到系統狀態的後概率分布，其中計算過程如下：

結合先驗概率分布下列觀測概率，我們可以得到系統狀態的後驗概率分布。

為了找到給定數據的最可能狀態，我們使用 MAP 估計(一種貝斯估計)，透過最大化後驗概率來尋找一組參數或狀態的最優值，而在高斯分布的假設下，**最大化後驗概率等同於下列成本函數(負對數似然函數)的最小化計算結果，因此我們必須最小化成本函數計算結果**。

$$
\mathop{\min}\limits_{\tilde{\rm x}^{\cal K}_k} \left( \Vert {\rm x}_k \boxminus \hat{\rm x}_k \Vert^2_{\hat{\rm P}_k} + \sum^m_{j = 1} \Vert {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k \Vert^2_{{\rm R}_j} \right)
$$

在上式中，成本函數 $\left( \Vert {\rm x}_k \boxminus \hat{\rm x}_k \Vert^2_{\hat{\rm P}_k} + \sum^m_{j = 1} \Vert {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k \Vert^2_{{\rm R}_j} \right)$ 表達了對狀態估計 $\hat{x}_k$ 的後驗概率最大化，其中 $\Vert {\rm x} \Vert^2_{\rm M} = {\rm x}^T {\rm M}^{-1} {\rm x}$。

- $\Vert {\rm x}_k \boxminus \hat{\rm x}_k \Vert^2_{\hat{\rm P}_k}$ 表示先驗誤差，即狀態估計誤差的平方，權重為先驗共變異數矩陣的逆矩陣 $\hat{P}_k^{-1}$
- $\sum^m_{j = 1} \Vert {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k \Vert^2_{{\rm R}_j}$：表示地 $j$ 次觀測的加權平方誤差，權重為觀測噪聲共變異數矩陣的逆矩陣 $\hat{R}_j^{-1}$

最小化這個成本函數代表著找到一個狀態估計，使得在綜合考慮先驗信息和新的觀測信息之後，這個估計的不確定性為最小，在 iterated Kalman Filter 的過程中，通過重複應用預測和更新步驟可以逐步縮小這個不確定性。

以上這個最大化後驗狀態估計的動作稱為 MAP (Maximum a-Posteriori) Estimate，Kalman Filter 的主要目的就是要解這個問題。

4. 利用 Kalman Filter 求解 MAP Estimate

我們可以透過 iterated Kalman Filter 來求解上述的最小目標函數，其中步驟說明如下：

- 計算卡爾曼增益 $K$

為了權衡觀測數據與當前估計的相對重要性，我們會需要計算當前狀態的卡爾曼增益，其中卡爾曼增益被定義為**使後驗共變異數最小化的值**。

根據卡爾曼濾波的推導過程，可以得到 $K$ 的最優解為：

$$
K = PH^T(HPH^T + R)^{-1}
$$

其中，$H$ 為觀測矩陣、$P$ 為先驗共變異數矩陣、$R$ 為觀測噪聲的共變異數矩陣。

以上方公式為基準，可以利用 Woodbury 矩陣恆等式推得 $K = (H^TR^{-1}H + P^{-1})^{-1} H^TR^{-1}$ 為 FAST-LIO2 狀態更新時使用的卡爾曼增益。

- 更新狀態矩陣與對應共變異數矩陣

已知卡爾曼濾波的標準更新方程如下：
* state update: $\hat{\rm x}_{t+1} = \hat{\rm x}_t + K_t(z_t - H {\rm x}_t)$
* covatiance matrix update: $P_{t+ 1} = (I - K_t H) P_t$

然而，在處理非線性系統時，這種直接的線性更新就不再適用，在這種情況下，我們需要採用另一種非線性策略。

在這個非線性環境下，狀態的更新公式變為一個在流形上的加法，以 $\boxplus$ 表示，而差分則以 $\boxminus$ 來表示。

因此我們可以得到以下的 optimal state 與其共變異數的計算結果：
* $\bar{\rm x}_k = \hat{\rm x}^{{\cal K} + 1}_k = \hat{\rm x}^{\cal K}_k \boxplus \left( -{\rm K}{\rm z}^{\cal K}_k - {\rm I} - {\rm KH})({\rm J}^{\cal K})^{-1} (\hat{\rm x}^{\cal K}_k \boxminus \hat{\rm x}_k) \right)$
* $\bar{\rm P}_k = ({\rm I-KH}){\rm P}$

而這個 optimal state 即為更新後的狀態向量，透過此更新的狀態向量 $\bar{\rm x}_k$，我們將第 $k$ 個 scan 中的每個 LiDAR point ($^L{\rm p}_j$) 根據以下公式轉換到 global frame 中：
$$
^G\bar{\rm p}_j = {^G\bar{\rm T}_{I_k}} {^I\bar{\rm T_{L_k}}} {^L{\rm p}_j}
$$
其中 $j = 1, ..., m$。

而最後轉換而得的 LiDAR point $\lbrace ^G\bar{\rm p}_j \rbrace$ 則會被加入到 ikd-Tree 中。
<!-- - 計算 optimal state 並將每個 LiDAR point 轉換到 global frame 中

在 residual computation 中，我們利用 measurement model 中建立的測量方程在 ${\rm x}^{\cal K}_k$ 處進行一次近似得到的結果 $0 = {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k + {\rm v}_j$ 可以調整為 $-{\rm v}_j = {\rm z}^{\cal K}_j + {\rm H}^{\cal K}_j \tilde{\rm x}^{\cal K}_k \sim {\cal N} (0, {\rm R}_j)$。

以上述計算結合 forward propagation 與 measurement model 中的計算結果，可以得到 state vector 的 maximum a-posteriori (MAP) [最大後驗估計，一個目標函數] 如下： -->

5. ikd tree 說明

上圖表示 FAST-LIO2 框架中對於地圖進行區域管理和為維護的示意圖，地圖點通過一個 ikd-tree 進行維護，通過不斷融合新的掃描點，地圖的區域也會不斷增大。但由於地圖不可無限制地增大，因此會在當前位置設定一個長度為 L 的局部區域，如上圖 (a) 所示。

檢測區域以 LiDAR 的位姿(位置)為中心、$r$ 為半徑的球，其中 $r = \gamma R$ ($R$ 為 LiDAR 的測距範圍，$\gamma$ 為一個大於 1 的鬆弛參數)。

當新的位置 $p'$ 產生的球狀範圍與地圖的邊緣相交集時，地圖會移動距離 $d = (\gamma - 1)R$，如上圖 (b) 所示，而橘色區域則會被通過盒狀式操作刪除。

- ikd tree 基本結構

k-d tree 是在 tree node 上除存一系列的點，而 ikd tree 則是一個 tree node 對應一個點雲，如上圖所示。

其中，相參數意義如下：

* `point`：除存了點雲的訊息，如座標、強度等
* `leftchild` / `rightchild`：指向左、右子樹的指標
* `axis`：用來劃分空間的分割軸
* `treesize`：針對當前 tree root 的子節點數量
* `range`：外接的一個立方體，由兩個對角點所組成，為每個維度的最大與最小座標

- ikd tree 的建構過程

idk tree 會不斷根據最常維度的終點進行空間劃分，直到子空間中只有一個點為止。下圖代表 ikd tree 建構的初始化資訊：


- ikd tree Incremental Updates

incremental operation 包含兩種類型，Point Insertion 與 Box-wise Delete。

* point insertion：表示在 ikd tree 中插入、刪除或重新插入一個單獨的點

如下方演算法所示，ikd tree 會同時進行點插入和地圖的 downsampling 操作。


根據上方演算法，首先會給定一個轉換到 global 座標系下的點 $p$，downsample resolution 為 $l$。

1. downsample operation：用算法將空間分隔程長度為 $l$ 的小立方體，找到包含點 $p$ 的小立方體 $C_D$，僅保存 $C_D$ 中最靠近中心的點。

首先在 ikd tree 上搜索所有包含在地方體 $C_D$ 的點，和新插入點一起存在一個數組 $V$ 中，之後比較數組 $V$ 中的所有點與 $C_D$ 中心點 $p_{center}$ 之間的距離，只保留 $V$ 中的最近點 $p_{nearest}$，並把保留點 $p_{nearest}$ 插入到 ikd tree 內。

2. point insertion：在 downsample operation 內，找到距離 $C_D$ 中心點 $p_{center}$ 的最近點 $p_{nearest}$ 後，需要將保留點 $p_{nearest}$ 以 point insertioin 的規則插入到 ikd tree 內。

第 11 ~ 24 行裡面以 recursion 的方式執行 point insertion，演算法中，從 root node 開始進行搜索，直到找到一個 empty node 後，就會新增新的 node。

在每一個 non-empty node 中，新的點和原先存於 tree node 上的點在分割維度上進行比較，以進一步去做 recursion。

插入過程中會訪問過節點的 treesize，並更新 range 屬性(line 21)，最後在第 22 行中檢查了插入點之後的 tree balance。

* box-wise delete：表示對一個立方體中的所有點進行插入、刪除或重新插入的操作

針對 ikd tree node 的刪除，作者採 lazy labels 的策略，也就是說點不會立即從 tree 上被移除，而是被標記為 "deleted"，而當 tree node `T` 的所有 child nodes 都被標記為 "deleted" 時，tree node `T` 的 `treedeleted` 會被標記為 `true`。

最後，在 reconstruct 過程中，被標記為 "deleted" 的點會從 tree 中被移除掉。

box-wise delete operation 依靠 `range` 屬性中的範圍訊息以及 tree node 中的 lazy labels 策略來實現。假設針對一個節點 `T`，它對應的空間範圍為立方體 $C_T$，需要從以節點 `T` 的子樹中刪除立方體 $C_O$ 中的點，則操作步驟具體如下：


1. 如果立方體 $C_O$ 和立方體 $C_T$ 之間沒有交集，則直接返回，不需要更新 ikd tree
2. 如果立方體 $C_O$ 完全包含立方體 $C_T$，則刪除 tree node `T`，設定

統整以上步驟，可得到以下的狀態更新演算法：