---
# System prepended metadata
title: 'FAST-LIO2: Fast Direct LiDAR-inertial Odometry'
tags: [SLAM, Paper Presentation Notes]
---
# 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
為了執行狀態估計，FAST-LIO2 繼承 FAST-LIO 的 tightly-coupled iterated Kalman Filter，即 LiDAR 新掃描到的點雲會通過 **tightly-coupled iterated Kalman Filter** 框架 (紅色虛線區)，加入到 global map 中，而 global map 中的地圖點則由以 incremental k-d Tree 為基礎建立而成的 **ikd-Tree** 來進行 (藍色虛線區)，若當前 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$：表示平面上的點
如上圖所示，紅色點代表 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 中。
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`，設定
統整以上步驟，可得到以下的狀態更新演算法：
