@
一、系統流程與符號說明
1、定義流形中的＋－
從上面的定義，很容易驗證
IEKF
| 符號 | 含義 |
|---|---|
| \(\mathbf{z}_{j}^{\kappa}=\mathbf{G}_{j}\left({ }^{G} \widehat{\mathbf{p}}_{f_{j}}^{\kappa}-{ }^{G} \mathbf{q}_{j}\right)\) | \(\mathbf{z}_{j}^{\kappa}\) LOAM的點到線、 點到面誤差作為殘差 |
| G() | 計算點到線、點到面殘差的函數 |
| \({ }^{G} \mathbf{q}_{j}\) | 真值 |
| \(\widehat{x}\) | IMU積分的當前位姿 |
| \(\widehat{\mathbf{P}}_{k}\) | IMU協方差 |
| \({ }^{L_{j}} \mathbf{n}_{f_{j}}\) | 雷達測量噪聲 |
| \({ }^{L_{j}} \mathbf{p}_{f_{j}}^{\mathrm{gt}}={ }^{L_{j}} \mathbf{p}_{f_{j}}-{ }^{L_{j}} \mathbf{n}_{f_{j}}\) | 雷達真值 = 測量值 - 噪聲 |
| T | 變換矩陣 |
觀察方程：
用它在\(\widehat{\mathbf{x}}_{k}^{\kappa}\)處的一階近似來逼近上面的方程會得到
\(\mathbf{H}_{j}^{\kappa}\) 是\(h_j()\)關於 \(\widetilde{\mathbf{x}}_{k}^{\kappa}\)的雅克比
\(v_j\) 測量噪聲，這里定義為了一個白噪聲
\(J^k\)是\(\left(\widehat{\mathbf{x}}_{k}^{\kappa} \mathbb{\square{}} \widetilde{\mathbf{x}}_{k}^{\kappa}\right) \square \widehat{\mathbf{x}}_{k}\) 的雅克比
迭代初值為： \(\widehat{\mathbf{x}}_{k}^{\kappa}=\widehat{\mathbf{x}}_{k}\), then \(\mathbf{J}^{\kappa}=\mathbf{I}\)
將(15)中的先驗與(14)中的后驗分布相結合，得到最大后驗估計(MAP)：
得到迭代卡爾曼公式
R是雷達測量噪聲矩陣
重復上面的步驟直至收斂,得到
其中卡爾曼增益公式（18）可以用下面的公式替換，他們是等價的。這避免了對測量矩陣H求逆
算法步驟
