Fast-LIO理论学习

IMU离散状态传播

Fast-LIO的离散方程（公式(7)）考虑了旋转扰动时的雅可比，其推导如下：

1. 旋转
   $$\begin{array}{rl}\delta {\theta }_{i+1}& =Log({}_{I_{i+1}}^G{\hat{\mathbf{R}}}^T{}_{I_{i+1}}^G\mathbf{R})\\ & =Log(({}_{I_i}^G\hat{\mathbf{R}}\cdot Exp({\hat{\omega }}_i\Delta t{)}^T({}_{I_i}^G\hat{\mathbf{R}}\cdot Exp(\delta {\theta }_i)\cdot Exp({\omega }_i\Delta t)))\\ & \approx Log(Exp({\hat{\omega }}_i\Delta t{)}^{T}Exp(\delta {\theta }_i)Exp({\hat{\omega }}_i\Delta t)\cdot Exp(-\mathbf{J}r({\hat{\omega }}_i\Delta t)(\delta {\mathbf{b}}_{g_i}+{\mathbf{n}}_{g_i})\Delta t))\\ & \approx Exp(-{\hat{\omega }}_i\Delta t)\delta {\theta }_i-\mathbf{J}r({\hat{\omega }}_i\Delta t)\Delta t\delta {\mathbf{b}}_{g_i}-\mathbf{J}r({\hat{\omega }}_i\Delta t)\Delta t{\mathbf{n}}_{g_i}\end{array}$$
   这里使用了两个公式代换：$\mathbf{R}Exp(\mathbf{u}){\mathbf{R}}^T=Exp(\mathbf{Ru})$, $\omega =\hat{\omega }-{\mathbf{b}}_g-{\mathbf{n}}_g$
2. 速度
   $$\begin{array}{rl}\delta {}^G{\mathbf{v}}_{I_{i+1}}& ={}^G{\mathbf{v}}_{I_{i+1}}-{}^G{\hat{\mathbf{v}}}_{I_{i+1}}\\ & =\delta {}^G{\mathbf{v}}_{I_i}+{}^G{\hat{\mathbf{R}}}_{I_i}Exp(\delta {\theta }_i){\mathbf{a}}_i\Delta t-{}^G{\hat{\mathbf{R}}}_{I_i}{\hat{\mathbf{a}}}_i\Delta t+\delta {}^G\mathbf{g}\Delta t\\ & \approx \delta {}^G{\mathbf{v}}_{I_i}+{}^G{\hat{\mathbf{R}}}_{I_i}(\mathbf{I}+\lfloor \delta {\theta }_{i\times }\rfloor )({\hat{\mathbf{a}}}_i-\delta {\mathbf{b}}_{a_i}-{\mathbf{n}}_{a_i})\Delta t-{}^G{\hat{\mathbf{R}}}_{I_i}{\hat{\mathbf{a}}}_i\Delta t\\ & \approx -{}^G{\hat{\mathbf{R}}}_{I_i}\lfloor {\hat{\mathbf{a}}}_{i\times }\rfloor \Delta t\delta {\theta }_i+\delta {}^G{\mathbf{v}}_{I_i}-{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t\delta {\mathbf{b}}_{a_i}+\Delta t\delta {}^G\mathbf{g}-{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t\delta {\mathbf{n}}_{a_i}\end{array}$$
   此处$\mathbf{a}=\hat{\mathbf{a}}-{\mathbf{b}}_a-{\mathbf{n}}_a$，与角速度类似。
3. 平移
   $$\begin{array}{rl}\delta {}^G{\mathbf{p}}_{I_{i+1}}& ={}^G{\mathbf{p}}_{I_{i+1}}-{}^G{\hat{\mathbf{p}}}_{I_{i+1}}\\ & =\delta {}^G{\mathbf{p}}_{I_i}+\delta {}^G{\mathbf{v}}_{I_i}\Delta t+\frac{1}{2}{\mathbf{a}}_i\Delta t^2-\frac{1}{2}{\hat{\mathbf{a}}}_i\Delta t^2\\ & =\Delta t\delta {}^G{\mathbf{v}}_{I_i}+\delta {}^G{\mathbf{p}}_{I_i}-\frac{1}{2}\Delta t^2\delta {\mathbf{b}}_{a_i}-\frac{1}{2}\Delta t^2{\mathbf{n}}_{a_i}\end{array}$$
   NOTE: R²Live论文在公式(6)中采用了上述的误差定义，但似乎在转移矩阵中忽略了二次项。
4. 零偏
   $$\delta {\mathbf{b}}_{g_{i+1}}=\delta {\mathbf{b}}_{g_i}+{\mathbf{n}}_{g_i},\delta {\mathbf{b}}_{a_{i+1}}=\delta {\mathbf{b}}_{a_i}+{\mathbf{n}}_{a_i}$$
   NOTE: 此处噪声参数$\mathbf{n}$是离散值。
5. 重力加速度
   $$\delta {}^G{\mathbf{g}}_{i+1}=\delta {}^G{\mathbf{g}}_i$$

滤波更新

Fast-LIO采用最大后验估计(MAP)构建优化方程，表示为公式(17)：
$$\underset{{\tilde{\mathbf{x}}}_k^{\kappa }}{\min }\left({\Vert {\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k\Vert }_{{\hat{\mathbf{P}}}_k^{-1}}^2+\sum _{j=1}^m{\Vert {\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }\Vert }_{{\mathbf{R}}_j^{-1}}^2\right)$$
为了方便表示，Fast-LIO将第二项堆叠成了高维矩阵形式$||{\mathbf{z}}_k^{\kappa }+\mathbf{H}{\tilde{\mathbf{x}}}_k^{\kappa }|{|}_{{\mathbf{R}}^{-1}}^2$。

先验分布

对Fast-LIO公式(16)推导如下：
$${\mathbf{J}}^{\kappa }=\frac{\partial ({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa })\boxminus {\hat{\mathbf{x}}}_k)}{\partial {\tilde{\mathbf{x}}}_k}$$
对于旋转，具体表示为：
J θ κ = ∂ L o g ( I k G R ^ T I k G R κ ) ∂ δ θ κ = ∂ L o g ( I k G R ^ T I k G R ^ κ E x p ( δ θ κ ) ) ∂ δ θ κ ≈ ∂ { J r ( L o g ( I k G R ^ T I k G R ^ κ ) ) − 1 δ θ κ + L o g ( I k G R ^ T I k G R ^ κ ) } ∂ δ θ κ = J r ( L o g ( I k G R ^ T I k G R ^ κ ) ) − 1 = J r ( I k G R ^ κ ⊟ I k G R ^ ) − 1 \begin{aligned} \mathbf{J}^\kappa_\theta&=\frac{\partial Log({}^G_{I_k}\mathbf{\hat R}^{T}{}^G_{I_k}\mathbf{R}^\kappa)}{\partial \delta \theta^\kappa} \\ &=\frac{\partial Log({}^G_{I_k}\mathbf{\hat R}^{T}{}^G_{I_k}\mathbf{\hat R}^\kappa Exp(\delta \theta^\kappa))}{\partial \delta \theta^\kappa} \\ &\approx \frac{\partial \{ \mathbf{J}_r(Log({}^G_{I_k}\mathbf{\hat R}^{T}{}^G_{I_k}\mathbf{\hat R}^\kappa))^{-1}\delta \theta^\kappa+Log({}^G_{I_k}\mathbf{\hat R}^{T}{}^G_{I_k}\mathbf{\hat R}^\kappa)\}}{\partial \delta \theta^\kappa} \\ &=\mathbf{J}_r(Log({}^G_{I_k}\mathbf{\hat R}^{T}{}^G_{I_k}\mathbf{\hat R}^\kappa))^{-1} \\ &=\mathbf{J}_r({}^G_{I_k}\mathbf{\hat R}^\kappa \boxminus {}^G_{I_k}\mathbf{\hat R})^{-1} \end{aligned} Jθκ​​=∂δθκ∂Log(Ik​G​R^TIk​G​Rκ)​=∂δθκ∂Log(Ik​G​R^TIk​G​R^κExp(δθκ))​≈∂δθκ∂{Jr​(Log(Ik​G​R^TIk​G​R^κ))−1δθκ+Log(Ik​G​R^TIk​G​R^κ)}​=Jr​(Log(Ik​G​R^TIk​G​R^κ))−1=Jr​(Ik​G​R^κ⊟Ik​G​R^)−1​
此处利用了右乘BCH近似公式$Log(Exp(\varphi )Exp(\delta \varphi ))\approx {\mathbf{J}}_r(\varphi {)}^{-1}\delta \varphi +\varphi$，其余的状态量雅可比都是单位阵。

最大后验估计

获得${\mathbf{J}}^{\kappa }$之后，可以对第一项进行代换：${\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k\approx {\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k+{\mathbf{J}}^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }$，如此分离待优化变量${\tilde{\mathbf{x}}}_k^{\kappa }$。
展开优化方程，并对${\tilde{\mathbf{x}}}_k^{\kappa }$求导得到：
$${\tilde{\mathbf{x}}}_k^{\kappa }=({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}{)}^{-1}(-{\mathbf{H}}^T{\mathbf{R}}^{-1}{\mathbf{z}}_k^{\kappa }-({\mathbf{J}}^{\kappa }{)}^T{\hat{\mathbf{P}}}_k^{-1}({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k))$$
其中$\mathbf{P}=({\mathbf{J}}^{\kappa }{)}^{-1}{\hat{\mathbf{P}}}_k({\mathbf{J}}^{\kappa }{)}^{-T}$。利用SMW公式$(A^{-1}+B{D}^{-1}C{)}^{-1}=A-AB(D+CAB{)}^{-1}CA$，可转化为最终结果并以此迭代：
$$\begin{array}{rl}{\tilde{\mathbf{x}}}_k^{\kappa }& =-\mathbf{K}{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH})({\mathbf{J}}^{\kappa }{)}^{-1}({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k)\\ {\mathbf{x}}_k^{\kappa +1}& ={\mathbf{x}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }\end{array}$$
NOTE：IEKF与高斯牛顿法在相同的问题条件下是具有等价性的。

参考资料

1. FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter
2. R²LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping
3. FAST-LIO公式推导
4. Fast-LIO优化方程推导