《预积分总结与公式推导》学习笔记1

本博客是《预积分总结与公式推导》一文的部分学习笔记，记录了一些李群李代数概念，IMU模型与预积分定义等知识。原文写的很好，逻辑清晰，推导详细。但是推导细节过多，不容易把控行文主线。本文提取并记录了该文的脉络核心，略去了细节，便于日后参考查找。本博客只是原文前半段的内容，后续会继续更新。

关于李群流形的一些基本概念和性质

hat和vee运算符

$${\mathbf{\omega }}^{\wedge }=\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]=\mathbf{W}$$

$${\mathbf{W}}^{\vee }={\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]}^{\vee }=\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]=\mathbf{\omega }$$

hat运算符的性质

$${\mathbf{a}}^{\wedge }\cdot \mathbf{b}=-{\mathbf{b}}^{\wedge }\cdot \mathbf{a},\forall \mathbf{a},\mathbf{b}\in R^3$$

指数映射

$$\exp \left({\varphi }^{\wedge }\right)=\mathbf{I}+\frac{\sin (\Vert \overline{\varphi }\Vert )}{\Vert \varphi \Vert }{\varphi }^{\wedge }+\frac{1-\cos (\Vert \overline{\varphi }\Vert )}{\Vert \varphi {\Vert }^2}{\left({\varphi }^{\wedge }\right)}^2$$

一阶近似

$$\exp \left({\varphi }^{\wedge }\right)\approx \mathbf{I}+{\varphi }^{\wedge }$$

对数映射

$$\log (\mathbf{R})=\frac{\phi \cdot \left(\mathbf{R}-{\mathbf{R}}^T\right)}{2\sin (\phi )}$$

其中$\phi ={\cos }^{-1}\left(\frac{\mathrm{tr}(R)-1}{2}\right)$

大写指数和对数映射

$$\begin{array}{l}\mathrm{Exp}:R^3\ni \varphi \to \exp \left({\varphi }^{\wedge }\right)\in SO(3)\\ \mathrm{Log}:\mathrm{S}\mathrm{O}(3)\ni \mathbf{R}\to \log (\mathbf{R}{)}^{\vee }\in R^3\end{array}$$

一阶近似

$$\begin{gathered} \mathrm{Exp}(\varphi +\delta \varphi )\approx \mathrm{Exp}(\varphi )\cdot \mathrm{Exp}\left({\mathbf{J}}_r(\varphi )\cdot \delta \varphi \right) \\ \mathrm{Log}(\mathrm{Exp}(\varphi )\cdot \mathrm{Exp}(\delta \varphi ))=\varphi +{\mathbf{J}}_r^{-1}(\varphi )\cdot \delta \varphi \end{gathered}$$

指数映射的共轭性质

$$\mathbf{R}\cdot \mathrm{Exp}(\varphi )\cdot {\mathbf{R}}^T=\exp \left(\mathbf{R}{\varphi }^{\wedge }{\mathbf{R}}^T\right)=\mathrm{Exp}(\mathbf{R}\varphi )$$

等价形式：用于矩阵乘法交换

$$\mathrm{Exp}(\varphi )\cdot \mathbf{R}=\mathbf{R}\cdot \mathrm{Exp}\left({\mathbf{R}}^T\varphi \right)$$

反对称矩阵的相似性质

$$\begin{gathered} (\mathbf{R}\varphi {)}^{\wedge }=\mathbf{R}{\varphi }^{\wedge }{\mathbf{R}}^T \\ 矩阵乘法交换形式：{\varphi }^{\wedge }\mathbf{R}=\mathbf{R}({\mathbf{R}}^T\varphi {)}^{\wedge } \end{gathered}$$

IMU测量模型和运动模型

测量模型

$${\tilde{\mathbf{\omega }}}_{wb}^b(t)={\mathbf{\omega }}_{wb}^b(t)+{\mathbf{b}}_g(t)+{\mathbf{\eta }}_g(t)$$

翻译：角速度测量值=角速度真实值+角速度测量偏置+角速度测量噪声

加速度和角速度都是在body坐标系（IMU当前坐标系）下进行测量的，但是由于重力加速度是在world坐标系下表示的，加速度真实值也表示在world坐标系下，所以测量值和真实值之间需要body-world坐标变换。由于只是向量坐标变换，只需要乘上旋转量。
$${\mathbf{f}}^b(t)={\mathbf{R}}_b^{wT}\left({\mathbf{a}}^w-{\mathbf{g}}^w\right)+{\mathbf{b}}_a(t)+{\eta }_a(t)$$
翻译：body坐标系下的加速度测量值=加速度真实值减去重力加速度（world坐标系下），转换到body坐标系下后再加上加速度测量偏置+加速度测量噪声

另一种形式

真实值=测量值-测量偏置-测量噪声
$${\mathbf{\omega }}_{wb}^b(t)={\tilde{\mathbf{\omega }}}_{wb}^b(t)-{\mathbf{b}}_g(t)-{\mathbf{\eta }}_g(t)$$

$${\mathbf{a}}^w={\mathbf{R}}_b^w\left({\mathbf{f}}^b(t)-{\mathbf{b}}_a(t)-{\eta }_a(t)\right)+{\mathbf{g}}^w$$

运动模型

微分形式

$$\begin{gathered} {\dot{\mathbf{R}}}_b^w={\mathbf{R}}_b^w{\left({\mathbf{\omega }}_{wb}^b\right)}^{\wedge } \\ {\dot{\mathbf{v}}}^w={\mathbf{a}}^w \\ {\dot{\mathbf{p}}}^w={\mathbf{v}}^w \end{gathered}$$

翻译：旋转矩阵对时间的导数=旋转矩阵*旋转矩阵对应的李代数；速度对时间导数=角速度；位置对时间的导数=速度

欧拉积分的离散形式

$$\begin{array}{l}{\mathbf{R}}_{b(t+\Delta t)}^w={\mathbf{R}}_{b(t)}^w\mathrm{Exp}\left({\mathbf{\omega }}_{wb}^b(t)\cdot \Delta t\right)\\ {\mathbf{v}}^w(t+\Delta t)={\mathbf{v}}^w(t)+{\mathbf{a}}^w(t)\cdot \Delta t\\ {\mathbf{p}}^w(t+\Delta t)={\mathbf{p}}^w(t)+{\mathbf{v}}^w(t)\cdot \Delta t+\frac{1}{2}{\mathbf{a}}^w(t)\cdot \Delta t^2\end{array}$$

IMU的运动测量模型

将测量模型的第二种形式代入测量模型的离散形式：
$$\begin{gathered} \mathbf{R}(t+\Delta t)=\mathbf{R}(t)\cdot \mathrm{Exp}\left(\left(\tilde{\mathbf{\omega }}(t)-{\mathbf{b}}_g(t)-{\eta }_{gd}(t)\right)\cdot \Delta t\right) \\ \mathbf{v}(t+\Delta t)=\mathbf{v}(t)+\mathbf{R}(t)\cdot \left(\tilde{\mathbf{f}}(t)-{\mathbf{b}}_a(t)-{\eta }_{ad}(t)\right)\cdot \Delta t+\mathbf{g}\cdot \Delta t \\ \mathbf{p}(t+\Delta t)=\mathbf{p}(t)+\mathbf{v}(t)\cdot \Delta t+\frac{1}{2}\mathbf{g}\cdot \Delta t^2+\frac{1}{2}\mathbf{R}(t)\cdot \left(\tilde{\mathbf{f}}(t)-{\mathbf{b}}_a(t)-{\eta }_{ad}(t)\right)\cdot \Delta t^2 \end{gathered}$$
为了简化表示，上式省略了一些上下标，并对一些表示进行了调整。同时，使用离散噪声代替连续噪声，二者关系如下：
$$\begin{gathered} \mathrm{Cov}\left({\mathbf{\eta }}_{\mathrm{g}\mathrm{d}}(t)\right)=\frac{1}{\Delta t}\mathrm{Cov}\left({\mathbf{\eta }}_{\mathrm{g}}(t)\right) \\ \mathrm{Cov}\left({\mathbf{\eta }}_{ad}(t)\right)=\frac{1}{\Delta t}\mathrm{Cov}\left({\eta }_a(t)\right) \end{gathered}$$
用离散时间步$k$代替连续时间$t$，并进一步简化符号
$$\begin{array}{l}{\mathbf{R}}_{k+1}={\mathbf{R}}_k\cdot \mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g-{\mathbf{\eta }}_k^{gd}\right)\cdot \Delta t\right)\\ {\mathbf{v}}_{k+1}={\mathbf{v}}_k+{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t+\mathbf{g}\cdot \Delta t\\ {\mathbf{p}}_{k+1}={\mathbf{p}}_k+{\mathbf{v}}_k\cdot \Delta t+\frac{1}{2}\mathbf{g}\cdot \Delta t^2+\frac{1}{2}{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\end{array}$$

IMU预积分

IMU区间离散积分

由时间区间$[i,j-1]$内的IMU测量，偏置和噪声，可以积分得到时间步$j$的IMU旋转，速度和位移量。
$$\begin{gathered} {\mathbf{R}}_j={\mathbf{R}}_i\cdot \prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g-{\eta }_k^{gd}\right)\cdot \Delta t\right) \\ {\mathbf{v}}_j={\mathbf{v}}_i+\mathbf{g}\cdot \Delta t_{ij}+\sum _{k=i}^{j-1}{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t \\ {\mathbf{p}}_j={\mathbf{p}}_i+\sum _{k=i}^{j-1}\left[{\mathbf{v}}_k\cdot \Delta t+\frac{1}{2}\mathbf{g}\cdot \Delta t^2+\frac{1}{2}{\mathbf{R}}_k\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\right] \end{gathered}$$

IMU预积分定义

虽然有了区间离散积分，但是起点变化后，需要从头开始积分。因此，我们想得到离散积分的增量形式。预积分定义了下述三个增量：
$$\begin{gathered} \Delta {\mathbf{R}}_{ij}\triangleq {\mathbf{R}}_i^T{\mathbf{R}}_j=\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g-{\mathbf{\eta }}_k^{gd}\right)\cdot \Delta t\right) \\ \Delta {\mathbf{v}}_{ij}\triangleq {\mathbf{R}}_i^T\left({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\cdot \Delta t_{ij}\right)=\sum _{k=i}^{j-1}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t \\ \Delta {\mathbf{p}}_{ij}\triangleq {\mathbf{R}}_i^T\left({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\cdot \Delta t_{ij}-\frac{1}{2}\mathbf{g}\cdot \Delta t_{ij}^2\right) \\ =\sum _{k=i}^{j-1}\left[\Delta {\mathbf{v}}_{ik}\cdot \Delta t+\frac{1}{2}\Delta {\mathbf{R}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a-{\eta }_k^{ad}\right)\cdot \Delta t^2\right] \end{gathered}$$
旋转增量的定义最好理解，直接左乘$i$时刻的旋转分量即可。速度和位置增量除了减去$i$时刻的速度和位移外，还额外减去了一部分重力加速度，这是为了在最终的增量结果中约去重力加速度分量。上式中位移增量的证明不容易直接得到，具体推导过程见参考文献[1]。

IMU预积分拆分

旋转拆分

$$\Delta {\mathbf{R}}_{ij}\triangleq \Delta {\tilde{\mathbf{R}}}_{ij}\cdot \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)$$

翻译：旋转预积分 (理想值) = 旋转预积分测量值 “减去“ 旋转预积分噪声值。

其中
$$\begin{gathered} \Delta {\tilde{\mathbf{R}}}_{ij}=\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g\right)\Delta t\right) \\ \mathrm{Exp}\left(-\delta {\varphi }_{ij}\right)=\prod _{k=i}^{j-1}\mathrm{Exp}\left(-\Delta {\tilde{\mathbf{R}}}_{k+1j}^T\cdot {\mathbf{J}}_r\left(\left({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_k^g\right)\Delta t\right)\cdot {\mathbf{\eta }}_k^{gd}\Delta t\right) \end{gathered}$$
翻译：旋转预积分测量值与IMU角速度测量，偏置有关；旋转预积分噪声值与IMU角速度测量值，偏置，噪声，以及旋转预积分测量值有关。

速度拆分

$$\Delta {\mathbf{v}}_{ij}\triangleq \Delta {\tilde{\mathbf{v}}}_{ij}-\delta {\mathbf{v}}_{ij}$$

翻译：速度预积分 (理想值) = 速度预积分测量值减去速度预积分噪声值。

其中
$$\begin{gathered} \Delta {\tilde{\mathbf{v}}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a\right)\cdot \Delta t\right] \\ \delta {\mathbf{v}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^{ad}\Delta t-\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a\right)}^{\wedge }\cdot \delta {\varphi }_{ik}\cdot \Delta t\right] \end{gathered}$$
翻译：速度预积分测量值与IMU加速度测量，偏置；旋转预积分测量有关；速度预积分噪声值与IMU加速度测量，偏置，噪声；以及旋转预积分测量，噪声有关。

位置拆分

$$\Delta {\mathbf{p}}_{ij}\triangleq \Delta {\tilde{\mathbf{p}}}_{ij}-\delta {\mathbf{p}}_{ij}$$

翻译：位置预积分 (理想值) = 位置预积分测量值减去位置预积分噪声值。

其中
$$\begin{gathered} \Delta {\tilde{\mathbf{p}}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot \left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a\right)\Delta t^2\right] \\ \delta {\mathbf{p}}_{ij}\triangleq \sum _{k=i}^{j-1}\left[\delta {\mathbf{v}}_{ik}\Delta t-\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}\cdot {\left({\tilde{\mathbf{f}}}_k-{\mathbf{b}}_k^a\right)}^{\wedge }\delta {\varphi }_{ik}\Delta t^2+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}{\eta }_k^{ad}\Delta t^2\right] \end{gathered}$$
翻译：位置预积分测量值与IMU加速度测量，偏置；旋转和速度预积分测量有关；速度预积分噪声值与IMU加速度测量，偏置，噪声；旋转预积分测量，噪声；速度预积分噪声有关。

参考文献

邱笑晨，预积分总结与公式推导，2018