Reference:

1. 深藍(lán)學(xué)院-多傳感器融合
2. 多傳感器融合定位理論基礎(chǔ)

文章跳轉(zhuǎn)：

1. 多傳感器融合定位一-3D激光里程計其一：ICP
2. 多傳感器融合定位二-3D激光里程計其二：NDT
3. 多傳感器融合定位三-3D激光里程計其三：點云畸變補(bǔ)償
4. 多傳感器融合定位四-3D激光里程計其四：點云線面特征提取
5. 多傳感器融合定位五-點云地圖構(gòu)建及定位
6. 多傳感器融合定位六-慣性導(dǎo)航原理及誤差分析
7. 多傳感器融合定位七-慣性導(dǎo)航解算及誤差分析其一
8. 多傳感器融合定位八-慣性導(dǎo)航解算及誤差分析其二
9. 多傳感器融合定位九-基于濾波的融合方法Ⅰ其一
10. 多傳感器融合定位十-基于濾波的融合方法Ⅰ其二
11. 多傳感器融合定位十一-基于濾波的融合方法Ⅱ
12. 多傳感器融合定位十二-基于圖優(yōu)化的建圖方法其一
13. 多傳感器融合定位十三-基于圖優(yōu)化的建圖方法其二
14. 多傳感器融合定位十四-基于圖優(yōu)化的定位方法
15. 多傳感器融合定位十五-多傳感器時空標(biāo)定(綜述)

1. 編碼器運動模型及標(biāo)定

1.1 編碼器基礎(chǔ)知識

編碼器感應(yīng)輪子的旋轉(zhuǎn)，并在旋轉(zhuǎn)時輸出脈沖，脈沖數(shù)與轉(zhuǎn)過的角度呈線性比例關(guān)系。

脈沖對應(yīng)的是角度增量，有時也用增量除以時間，轉(zhuǎn)成輪子轉(zhuǎn)動的角速度輸出。

需要注意的是，編碼器只是各種轉(zhuǎn)角測量方式中的一種，其他還有輪速計、霍爾傳感器等，本課程以編碼器為例子講解模型，但同樣適用于其他形式的傳感器。

編碼器安裝方式有單輪、雙輪、三輪，本課程推導(dǎo)僅圍繞雙輪差分模型進(jìn)行展開。
該模型中，需要用到以下變量：

• ${\mathbf{r}}_L$：左輪半徑
• ${\mathbf{r}}_R$：右輪半徑
• $\mathbf{d}$：輪子離底盤中心的距離
• ${\mathbf{\omega }}_L$：左輪自轉(zhuǎn)角速度
• ${\mathbf{\omega }}_{\mathbf{R}}$：右輪自轉(zhuǎn)角速度
• ${\mathbf{v}}_L$：左輪線速度
• ${\mathbf{v}}_R$：右輪線速度

實際使用時，標(biāo)定完成后， ${\mathbf{r}}_L$、${\mathbf{r}}_R$、$\mathbf{d}$ 為固定參數(shù)， ${\mathbf{\omega }}_L$ 和 ${\mathbf{\omega }}_R$ 為測量值，而 ${\mathbf{v}}_L$ 和 ${\mathbf{v}}_R$ 可以通過下式計算得到：
$$\begin{array}{rl}{\mathbf{v}}_L& ={\mathbf{\omega }}_L{\mathbf{r}}_L\\ {\mathbf{v}}_R& ={\mathbf{\omega }}_R{\mathbf{r}}_R\end{array}$$

1.2 編碼器運動模型

運動模型的作用是，使用前述已知量，求解以下變量：
$\mathbf{\omega }$ : 底盤中心的角速度
$\mathbf{v}$ : 底盤中心的線速度
$\mathbf{r}$ : 底盤中心圓弧運動旋轉(zhuǎn)半徑

1.2.1 旋轉(zhuǎn)半徑求解

雙輪差分模型下，左右輪圓弧運動的角速度相等，且等于底盤中心圓弧運動的角速度(兩個輪子的自轉(zhuǎn)角速度是相同的)，因此有：
$$\mathbf{\omega }=\frac{{\mathbf{v}}_L}{\mathbf{r}?\mathbf{d}}=\frac{{\mathbf{v}}_R}{\mathbf{r}+\mathbf{d}}$$由此可以得出：
$${\mathbf{v}}_L(\mathbf{r}+\mathbf{d})={\mathbf{v}}_R(\mathbf{r}?\mathbf{d})$$移項可得：
$$\left({\mathbf{v}}_R?{\mathbf{v}}_L\right)\mathbf{r}=\left({\mathbf{v}}_R+{\mathbf{v}}_L\right)\mathbf{d}$$從而可以得到：
$$\mathbf{r}=\frac{{\mathbf{v}}_R+{\mathbf{v}}_L}{{\mathbf{v}}_R?{\mathbf{v}}_L}\mathbf{d}$$

1.2.2 角速度求解

把旋轉(zhuǎn)半徑的求解結(jié)果，代入角速度公式，即可得到：
$$\mathbf{\omega }=\frac{{\mathbf{v}}_L}{\frac{{\mathbf{v}}_R+{\mathbf{v}}_L}{{\mathbf{v}}_R?{\mathbf{v}}_L}\mathbf{d}?\mathbf{d}}=\frac{{\mathbf{v}}_R?{\mathbf{v}}_L}{2\mathbf{d}}$$

1.2.3 線速度求解

利用旋轉(zhuǎn)角速度和旋轉(zhuǎn)半徑的結(jié)果，可以直接得到線速度：
$$\mathbf{v}=\mathbf{\omega }\mathbf{r}=\frac{{\mathbf{v}}_R?{\mathbf{v}}_L}{2\mathbf{d}}\frac{{\mathbf{v}}_R+{\mathbf{v}}_L}{{\mathbf{v}}_R?{\mathbf{v}}_L}\mathbf{d}=\frac{{\mathbf{v}}_R+{\mathbf{v}}_L}{2}$$

1.2.4 位姿求解

假設(shè) ${\mathbf{x}}_k,{\mathbf{y}}_k,{\mathbf{\theta }}_k$ 為當(dāng)前時刻位姿，${\mathbf{x}}_{k?1},{\mathbf{y}}_{k?1},{\mathbf{\theta }}_{k?1}$ 為上一時刻的位姿，則有：
$$\begin{array}{rl}& {\mathbf{\theta }}_k={\mathbf{\theta }}_{k?1}+\mathbf{\omega }\Delta t\\ & {\mathbf{x}}_k={\mathbf{x}}_{k?1}+\mathbf{v}\Delta t\cos \left({\mathbf{\theta }}_{\mathbf{k}?1}\right)\\ & {\mathbf{y}}_k={\mathbf{y}}_{k?1}+\mathbf{v}\Delta t\sin \left({\mathbf{\theta }}_{\mathbf{k}?1}\right)\end{array}$$其中：
$$\Delta t=t_k?t_{k?1}$$

1.3 編碼器的標(biāo)定

標(biāo)定可以理解為運動模型求解過程的反向過程，具體是指在已知底盤中心線速度、角速度的情況下，求解輪子半徑、輪子離底盤中心距離等。
已知量：
$v$：底盤中心的線速度
$\omega$：底盤中心的角速度
待求解量：
${\mathbf{r}}_L$：左輪半徑
${\mathbf{r}}_R$：右輪半徑
$\mathbf{d}$：輪子離底盤中心的距離
實際標(biāo)定時，線速度、角速度由其他傳感器提供(比如雷達(dá)點云和地圖匹配)，且為了簡化模型，認(rèn)為雷達(dá)裝在底盤中心正上方。(這里雷達(dá)最好的方法是先建好一個點云地圖，然后在點云地圖里面匹配，然后拿它做觀測，去提供線速度和角速度的結(jié)果，而不要用激光里程計-----里程計本身就是有累計誤差的)

1.3.1 輪子半徑標(biāo)定

由于速度的求解公式為：
$$\mathbf{v}=\frac{{\mathbf{v}}_R+{\mathbf{v}}_L}{2}=\frac{{\mathbf{\omega }}_R{\mathbf{r}}_R+{\mathbf{\omega }}_L{\mathbf{r}}_L}{2}$$它可以重新寫為：
$$\left[\begin{array}{ll}{\mathbf{\omega }}_R& {\mathbf{\omega }}_L\end{array}\right]\left[\begin{array}{l}{\mathbf{r}}_R\\ {\mathbf{r}}_L\end{array}\right]=2\mathbf{v}$$當(dāng)有多組測量值時，可以構(gòu)成如下方程組：
$$\left[\begin{array}{cc}{\mathbf{\omega }}_{R0}& {\mathbf{\omega }}_{L0}\\ {\mathbf{\omega }}_{R1}& {\mathbf{\omega }}_{L1}\\ ?& ?\\ {\mathbf{\omega }}_{RN}& {\mathbf{\omega }}_{LN}\end{array}\right]\left[\begin{array}{l}{\mathbf{r}}_R\\ {\mathbf{r}}_L\end{array}\right]=\left[\begin{array}{c}2{\mathbf{v}}_0\\ 2{\mathbf{v}}_1\\ ?\\ 2{\mathbf{v}}_N\end{array}\right]$$這是典型的最小二乘問題，可用最小二乘標(biāo)準(zhǔn)形式計算。

1.3.2 輪子與底盤中心距離標(biāo)定

由于角速度的求解公式為：
$$\mathbf{\omega }=\frac{{\mathbf{v}}_R?{\mathbf{v}}_{\mathbf{L}}}{2\mathbf{d}}$$在經(jīng)過輪子半徑標(biāo)定之后，分子上的兩項可認(rèn)為是已知量，因此可以得到：
$$\mathbf{d}=\frac{{\mathbf{v}}_R?{\mathbf{v}}_L}{2\mathbf{\omega }}$$雖然可直接求解，但是為了抑制噪聲帶來的影響，因多次采樣計算取平均。

2. 融合編碼器的濾波方法

2.1 核心思路

在上一節(jié)課濾波模型的基礎(chǔ)上增加編碼器進(jìn)行融合，有一種非常簡單的方法，即使用編碼器解算的速度作為觀測量，加入原來模型的觀測方程中，而其他環(huán)節(jié)保持不變。

2.2 觀測量定義

編碼器提供的是載體系下的速度觀測，在前 $(\mathrm{x})$-左 $(\mathrm{y})$-上(z)坐標(biāo)系的定義下，$\mathrm{x}$ 方向的速度分量是已知的 ${\mathbf{v}}_x^b={\mathbf{v}}_m$。 另外，在以車作為載體的情況下，由于車的側(cè)向和天向沒有運動，因此又有 ${\mathbf{v}}_y^b=0$，${\mathbf{v}}_z^b=0$。
基于此，我們可以認(rèn)為 $b$ 系 3 個維度的速度分量都是可觀測的。

2.3 觀測方程推導(dǎo)

由于導(dǎo)航解算得到的是 $w$ 系下得速度，而速度觀測是 $b$ 系下得，因此需要推導(dǎo)二者之間的誤差關(guān)系，才能得到相應(yīng)的觀測方程。
推導(dǎo)方法仍按照第6講的固定套路進(jìn)行。

1. 寫出不考慮誤差時的方程：
   $${\mathbf{v}}^b={\mathbf{R}}_{bw}{\mathbf{v}}^w$$
2. 寫出考慮誤差時的方程：
   $${\tilde{\mathbf{v}}}^b={\tilde{\mathbf{R}}}_{bw}{\tilde{\mathbf{v}}}^w$$
3. 寫出真實值與理想值之間的關(guān)系：
   $$\begin{array}{rl}& {\tilde{\mathbf{v}}}^b={\mathbf{v}}^b+\delta {\mathbf{v}}^b\\ & {\tilde{\mathbf{v}}}^w={\mathbf{v}}^w+\delta {\mathbf{v}}^w\\ & {\tilde{\mathbf{R}}}_{bw}={\tilde{\mathbf{R}}}_{wb}^T={\left({\mathbf{R}}_{wb}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\right)}^T\\ & =\left(\mathbf{I}?[\delta \mathbf{\theta }{]}_{\times }\right){\mathbf{R}}_{bw}\end{array}$$
4. 把3)中的關(guān)系帶入2)式：
   $${\mathbf{v}}^b+\delta {\mathbf{v}}^b=\left(\mathbf{I}?[\delta \mathbf{\theta }{]}_{\times }\right){\mathbf{R}}_{bw}\left({\mathbf{v}}^w+\delta {\mathbf{v}}^w\right)$$
5. 把1)中的關(guān)系帶入4)式：
   $${\mathbf{R}}_{bw}{\mathbf{v}}^w+\delta {\mathbf{v}}^b=\left(\mathbf{I}?[\delta \mathbf{\theta }{]}_{\times }\right){\mathbf{R}}_{bw}\left({\mathbf{v}}^w+\delta {\mathbf{v}}^w\right)$$
6. 化簡方程：
   $$\begin{array}{rl}\delta {\mathbf{v}}^b& ={\mathbf{R}}_{bw}\delta {\mathbf{v}}^w?[\delta \mathbf{\theta }{]}_{\times }{\mathbf{R}}_{bw}{\mathbf{v}}^w\\ & ={\mathbf{R}}_{bw}\delta {\mathbf{v}}^w?[\delta \mathbf{\theta }{]}_{\times }{\mathbf{v}}^b\\ & ={\mathbf{R}}_{bw}\delta {\mathbf{v}}^w+{\left[{\mathbf{v}}^b\right]}_{\times }\delta \mathbf{\theta }\end{array}$$

根據(jù)前一章內(nèi)容，狀態(tài)量為
$$\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_{\omega }\end{array}\right]$$而融合編碼器以后，觀測量變?yōu)?br /> $$\mathbf{y}=\left[\begin{array}{c}\delta \overline{\mathbf{p}}\\ \delta {\overline{\mathbf{v}}}^b\\ \delta \overline{\mathbf{\theta }}\end{array}\right]$$其中 $\delta {\overline{\mathbf{v}}}^b$ 的觀測值可以通過下式獲得
$$\delta {\overline{\mathbf{v}}}_b={\tilde{\mathbf{v}}}^b?{\mathbf{v}}^b={\tilde{\mathbf{R}}}_{bw}{\tilde{\mathbf{v}}}^w?\left[\begin{array}{c}{\mathbf{v}}_m\\ 0\\ 0\end{array}\right]$$此時的觀測方程 $\mathbf{y}={\mathbf{G}}_t\delta \mathbf{x}+{\mathbf{C}}_t\mathbf{n}$ 中的各變量應(yīng)重新寫為
$$\begin{array}{rl}& {\mathbf{G}}_t=\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0& 0& 0& 0\\ 0& {\mathbf{R}}_{bw}& {\left[{\mathbf{v}}^b\right]}_{\times }& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0& 0\end{array}\right]\\ & {\mathbf{C}}_t=\left[\begin{array}{ccc}{\mathbf{I}}_3& 0& 0\\ 0& {\mathbf{I}}_3& 0\\ 0& 0& {\mathbf{I}}_3\end{array}\right]\\ & \mathbf{n}={\left[\begin{array}{lllllllll}{n}_{\delta {\bar{p}}_x}& n_{\delta {\bar{p}}_y}& n_{\delta {\bar{p}}_z}& n_{\delta {\bar{v}}_x^b}& n_{\delta {\bar{v}}_y^b}& n_{\delta {\bar{v}}_z^b}& n_{\delta {\bar{\theta }}_x}& n_{\delta {\bar{\theta }}_y}& n_{\delta {\bar{\theta }}_z}\end{array}\right]}^T\end{array}$$隨后，便可以使用新的觀測方程，不改變其他方程，直接按照原有流程進(jìn)行Kalman濾波融合。

3. 融合運動約束的濾波方法

很多時候，硬件平臺并沒有編碼器，不能直接使用上一小節(jié)的模型，但是車本身的運動特性(即側(cè)向速度和天向速度為0)仍然可以使用。

它對觀測量帶來的改變僅僅是少了一個維度($x$ 方向)，而推導(dǎo)方法并沒有改變，因此此處直接給出該融合模式下的推導(dǎo)結(jié)果。

新的觀測量為
$$\mathbf{y}=\left[\begin{array}{c}\delta \overline{\mathbf{p}}\\ {\left[\delta {\overline{\mathbf{v}}}^b\right]}_{yz}\\ \delta \overline{\mathbf{\theta }}\end{array}\right]$$$[?{]}_{yz}$ 表示只取三維向量或矩陣的后2行

此時的觀測方程 $\mathbf{y}={\mathbf{G}}_t\delta \mathbf{x}+{\mathbf{C}}_t\mathbf{n}$ 中的各變量應(yīng)重新寫為
$$\begin{array}{rl}& {\mathbf{G}}_t=\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0& 0& 0& 0\\ 0& {\left[{\mathbf{R}}_{bw}\right]}_{yz}& {\left[{\left[{\mathbf{v}}^b\right]}_{\times }\right]}_{yz}& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0& 0\end{array}\right]\\ & {\mathbf{C}}_t=\left[\begin{array}{ccc}{\mathbf{I}}_3& 0& 0\\ 0& {\mathbf{I}}_2& 0\\ 0& 0& {\mathbf{I}}_3\end{array}\right]\\ & \mathbf{n}={\left[\begin{array}{llllllll}{n}_{\delta {\bar{p}}_x}& n_{\delta {\bar{p}}_y}& n_{\delta {\bar{p}}_z}& n_{\delta {\bar{v}}_y^b}& n_{\delta {\bar{v}}_z^b}& n_{\delta {\bar{\theta }}_x}& n_{\delta {\bar{\theta }}_y}& n_{\delta {\bar{\theta }}_z}\end{array}\right]}^T\end{array}$$隨后的Kalman流程仍然與之前保持一致。

4. 融合點云特征的濾波方法

4.1 整體思路

以IMU做狀態(tài)預(yù)測，以特征中的點-面距離、點-線距離為約束(觀測)，修正誤差。

論文題目：LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation

4.2 濾波模型

1. 狀態(tài)定義
   位姿定義：
   $${\mathbf{x}}_w^{b_k}:=\left[{\mathbf{p}}_w^{b_k},{\mathbf{q}}_w^{b_k}\right]$$相對位姿相關(guān)：
   $${\mathbf{x}}_{b_{k+1}}^{b_k}:=\left[{\mathbf{p}}_{b_{k+1}}^{b_k},{\mathbf{v}}_{b_{k+1}}^{b_k},{\mathbf{q}}_{b_{k+1}}^{b_k},{\mathbf{b}}_a,{\mathbf{b}}_g,{\mathbf{g}}^{b_k}\right]$$狀態(tài)量：
   $$\delta \mathbf{x}:=\left[\delta \mathbf{p},\delta \mathbf{v},\delta \mathbf{\theta },\delta {\mathbf{b}}_a,\delta {\mathbf{b}}_g,\delta \mathbf{g}\right]$$狀態(tài)量修正：
   $${\mathbf{x}}_{b_{k+1}}^{b_k}={}^{?}{\mathbf{x}}_{b_{k+1}}^{b_k}?\mathbf{\delta }\mathbf{x}=\left[\begin{array}{c}?{\mathbf{p}}_{b_{k+1}}^{b_k}+\mathbf{\delta }\mathbf{p}\\ ?{\mathbf{v}}_k^{b_k}+\mathbf{\delta }\mathbf{v}\\ ?{\mathbf{q}}_{b_{k+1}}^{b_k}?\exp (\mathbf{\delta }\mathbf{\theta })\\ ?{\mathbf{b}}_a+\mathbf{\delta }{\mathbf{b}}_a\\ ?{\mathbf{b}}_g+\delta {\mathbf{b}}_g\\ ?{\mathbf{g}}^{b_k}+\mathbf{\delta }\mathbf{g}\end{array}\right]$$其中 ${}^{?}{\mathbf{x}}_{b_{k+1}}^{b_k}$ 表示預(yù)測值。

2. 狀態(tài)方程
   $$\delta \dot{\mathbf{x}}(t)={\mathbf{F}}_t\delta \mathbf{x}(t)+{\mathbf{G}}_t\mathbf{w}$$其中
   $$\begin{array}{rl}{\mathbf{F}}_t& =\left[\begin{array}{cccccc}0& \mathbf{I}& 0& 0& 0& 0\\ 0& 0& ?{\mathbf{R}}_t^{b_k}{\left[{\hat{\mathbf{a}}}_t\right]}_{\times }& ?{\mathbf{R}}_t^{b_k}& 0& ?{\mathbf{I}}_3\\ 0& 0& ?{\left[{\hat{\omega }}_t\right]}_{\times }& 0& ?{\mathbf{I}}_3& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]{\mathbf{G}}_t=\left[\begin{array}{cccc}0& 0& 0& 0\\ ?{\mathbf{R}}_t^{b_k}& 0& 0& 0\\ 0& ?{\mathbf{I}}_3& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0\\ 0& 0& 0& {\mathbf{I}}_3\\ 0& 0& 0& 0\end{array}\right]\\ \mathbf{w}& ={\left[{\mathbf{n}}_a^T,{\mathbf{n}}_g^T,{\mathbf{n}}_{b_a}^T,{\mathbf{n}}_{b_g}^T\right]}^T\end{array}$$

3. 觀測方程
   觀測的計算與loam中前后幀匹配的思想一致，都是計算 點-面、點-線的殘差
   其中
   $${\hat{\mathbf{p}}}_i^{l_k}={\left({\mathbf{R}}_l^b\right)}^T\left({\mathbf{R}}_{b_{k+1}}^{b_k}\left({\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right)+{\mathbf{p}}_{b_{k+1}}^{b_k}?{\mathbf{p}}_l^b\right)$$為了計算觀測方程，需要計算殘差對狀態(tài)量的雅可比
   $${\mathbf{H}}_k=\frac{?\mathbf{f}}{?{\hat{\mathbf{p}}}_i^{l_k}}?\frac{?{\hat{\mathbf{p}}}_i^{l_k}}{?\delta \mathbf{x}}$$上式中包含兩部分，第一部分已經(jīng)講過，此處只推導(dǎo)第 二部分。
   除了旋轉(zhuǎn)和平移外，殘差項對其它量的導(dǎo)數(shù)均為 0 。
   a.對平移求導(dǎo)
   $$\begin{array}{rl}& \frac{?{\hat{\mathbf{p}}}_i^{l_k}}{?\delta \mathbf{p}}\\ =& \frac{?\left[{\left({\mathbf{R}}_l^b\right)}^{?}\left({\mathbf{R}}_{b_{k+1}}^{b_k}\left({\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right)+{\mathbf{p}}_{b_{k+1}}^{b_k}?{\mathbf{p}}_l^b\right)\right]}{?\delta \mathbf{p}}\\ =& \frac{?\left[{\left({\mathbf{R}}_l^b\right)}^{?}{\mathbf{p}}_{b_{k+1}}^{b_k}\right]}{?\delta \mathbf{p}}\\ =& {\left({\mathbf{R}}_l^b\right)}^{?}\end{array}$$b.對旋轉(zhuǎn)求導(dǎo)
   $$\begin{array}{rl}& \frac{?{\hat{\mathbf{p}}}_i^{l_k}}{?\delta \mathbf{x}}\\ =& \frac{?\left[{\left({\mathbf{R}}_l^b\right)}^T\left({\mathbf{R}}_{b_{k+1}}^{b_k}\left({\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right)+{\mathbf{p}}_{b_{k+1}}^{b_k}?{\mathbf{p}}_l^b\right)\right]}{?\delta \mathbf{\theta }}\\ =& {\left({\mathbf{R}}_l^b\right)}^T\frac{?\left[{\mathbf{R}}_{b_{k+1}}^{b_k}\left({\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right)\right]}{?{\mathbf{R}}_{b_{k+1}}^{b_k}}\frac{?{\mathbf{R}}_{b_{k+1}}^{b_k}}{?\delta \mathbf{\theta }}\\ =& ?{\left({\mathbf{R}}_l^b\right)}^T{\left[{\mathbf{R}}_{b_{k+1}}^{b_k}\left({\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right)\right]}_{\times }{\mathbf{R}}_{b_{k+1}}^{b_k}{\mathbf{J}}_r^{?1}(\mathbf{\theta })\\ =& ?{\left({\mathbf{R}}_l^b\right)}^T{\mathbf{R}}_{b_{k+1}}^{b_k}{\left[{\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right]}_{\times }{\mathbf{R}}_{b_k}^{b_{k+1}}{\mathbf{R}}_{b_{k+1}}^{b_k}{\mathbf{J}}_r^{?1}(\mathbf{\theta })\\ =& ?{\left({\mathbf{R}}_l^b\right)}^T{\mathbf{R}}_{b_{k+1}}^{b_k}{\left[{\mathbf{R}}_l^b{\mathbf{p}}_i^{l_{k+1}}+{\mathbf{p}}_l^b\right]}_{\times }{\mathbf{J}}_r^{?1}(\mathbf{\theta })\end{array}$$預(yù)測 :
   $$\begin{array}{rl}& \delta {\mathbf{x}}_{t_{\tau }}=\left(\mathbf{I}+{\mathbf{F}}_{t_{\tau }}\Delta t\right)\delta {\mathbf{x}}_{t_{\tau ?1}}\\ & {\mathbf{P}}_{t_{\tau }}=\left(\mathbf{I}+{\mathbf{F}}_{t_{\tau }}\Delta t\right){\mathbf{P}}_{t_{\tau ?1}}{\left(\mathbf{I}+{\mathbf{F}}_{t_{\tau }}\Delta t\right)}^T+\left({\mathbf{G}}_{i_{\tau }}\Delta t\right)\mathbf{Q}{\left({\mathbf{G}}_{t_t}\Delta t\right)}^T\\ & {\mathbf{K}}_{k,j}={\mathbf{P}}_k{\mathbf{H}}_{k,j}^T{\left({\mathbf{H}}_{k,j}{\mathbf{P}}_k{\mathbf{H}}_{k,j}^T+{\mathbf{J}}_{k,j}{\mathbf{M}}_k{\mathbf{J}}_{k,j}^T\right)}^{?1}\end{array}$$迭代觀測: $\Delta {\mathbf{x}}_j={\mathbf{K}}_{k,j}\left({\mathbf{H}}_{k,j}\delta {\mathbf{x}}_j?f\left({}^{?}{\mathbf{x}}_{b_{k+1}}^{b_k}?\mathbf{\delta }{\mathbf{x}}_j\right)\right)$
   $$\delta {\mathbf{x}}_{j+1}=\delta {\mathbf{x}}_j+\Delta {\mathbf{x}}_j$$后驗方差: ${\mathbf{P}}_{k+1}=\left(\mathbf{I}?{\mathbf{K}}_{k,n}{\mathbf{H}}_{k,n}\right){\mathbf{P}}_k{\left(\mathbf{I}?{\mathbf{K}}_{k,n}{\mathbf{H}}_{k,n}\right)}^T+{\mathbf{K}}_{k,n}{\mathbf{M}}_k{\mathbf{K}}_{k,n}^T$

4.3 位姿更新

$${\mathbf{x}}_w^{b_{k+1}}=\left[\begin{array}{c}{\mathbf{p}}_w^{b_{k+1}}\\ {\mathbf{q}}_w^{b_{k+1}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_{b_k}^{b_{k+1}}\left({\mathbf{p}}_w^{b_k}?{\mathbf{p}}_{b_{k+1}}^{b_k}\right)\\ {\mathbf{q}}_{b_k}^{b_{k+1}}?{\mathbf{q}}_w^{b_k}\end{array}\right]$$

4.4 相似工作

論文題目：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter