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{{\mathbf{x}}_k^{\kappa }}{\min }\left({\Vert {\mathbf{x}}_k\boxminus {\mathbf{x}}_k\Vert }_{{\hat{\mathbf{P}}}_k^{-1}}^2+\sum _{j=1}^m{\Vert {\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\mathbf{x}}_k^{\kappa }\Vert }_{{\mathbf{R}}_j^{-1}}^2\right)$$
为了方便表示，Fast-LIO将第二项堆叠成了高维矩阵形式$||{\mathbf{z}}_k^{\kappa }+\mathbf{H}{\mathbf{x}}_k^{\kappa }|{|}_{{\mathbf{R}}^{-1}}^2$。

先验分布

对Fast-LIO公式(16)推导如下：
$${\mathbf{J}}^{\kappa }=\frac{\partial ({\mathbf{x}}_k^{\kappa }⊞{\mathbf{x}}_k^{\kappa })\boxminus {\mathbf{x}}_k)}{\partial {\mathbf{x}}_k}$$
对于旋转，具体表示为：
$$\begin{array}{rl}{\mathbf{J}}_{\theta }^{\kappa }& =\frac{\partial Log({}_{I_k}^G{\hat{\mathbf{R}}}^T{}_{I_k}^G{\mathbf{R}}^{\kappa })}{\partial \delta {\theta }^{\kappa }}\\ & =\frac{\partial Log({}_{I_k}^G{\hat{\mathbf{R}}}^T{}_{I_k}^G{\hat{\mathbf{R}}}^{\kappa }Exp(\delta {\theta }^{\kappa }))}{\partial \delta {\theta }^{\kappa }}\\ & \approx \frac{\partial \{{\mathbf{J}}_r(Log({}_{I_k}^G{\hat{\mathbf{R}}}^T{}_{I_k}^G{\hat{\mathbf{R}}}^{\kappa }){)}^{-1}\delta {\theta }^{\kappa }+Log({}_{I_k}^G{\hat{\mathbf{R}}}^T{}_{I_k}^G{\hat{\mathbf{R}}}^{\kappa })\}}{\partial \delta {\theta }^{\kappa }}\\ & ={\mathbf{J}}_r(Log({}_{I_k}^G{\hat{\mathbf{R}}}^T{}_{I_k}^G{\hat{\mathbf{R}}}^{\kappa }){)}^{-1}\\ & ={\mathbf{J}}_r({}_{I_k}^G{\hat{\mathbf{R}}}^{\kappa }\boxminus {}_{I_k}^G\hat{\mathbf{R}}{)}^{-1}\end{array}$$
此处利用了右乘BCH近似公式$Log(Exp(\varphi )Exp(\delta \varphi ))\approx {\mathbf{J}}_r(\varphi {)}^{-1}\delta \varphi +\varphi$，其余的状态量雅可比都是单位阵。

最大后验估计

获得${\mathbf{J}}^{\kappa }$之后，可以对第一项进行代换：${\mathbf{x}}_k\boxminus {\mathbf{x}}_k\approx {\mathbf{x}}_k^{\kappa }\boxminus {\mathbf{x}}_k+{\mathbf{J}}^{\kappa }{\mathbf{x}}_k^{\kappa }$，如此分离待优化变量${\mathbf{x}}_k^{\kappa }$。
展开优化方程，并对${\mathbf{x}}_k^{\kappa }$求导得到：
$${\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}({\mathbf{x}}_k^{\kappa }\boxminus {\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}{\mathbf{x}}_k^{\kappa }& =-\mathbf{K}{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH})({\mathbf{J}}^{\kappa }{)}^{-1}({\mathbf{x}}_k^{\kappa }\boxminus {\mathbf{x}}_k)\\ {\mathbf{x}}_k^{\kappa +1}& ={\mathbf{x}}_k^{\kappa }⊞{\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优化方程推导