从IMU预积分理解最大后验概率问题及李代数应用

发表于 2023-06-12 更新于 2024-09-19 分类于 SLAM ， Theory 阅读次数： Changyan：

本文内容

为了能够更深刻地理解

后端优化中的最小二乘问题
李代数在定位后端的应用

这两个问题，本文使用IMU预积分作为一个实际问题来联系上面两个内容。IMU预积分可以在欧拉角表达下进行，但是更优的方法是在流形（特殊正交群\(SO(3)\)是一种流形）上进行。因此，本文使用论文《On-Manifold Preintegration for Real-TimeVisual-Inertial Odometry》提出的IMU预积分方法作为应用实例，在讲述方法的同时将上面两个问题结合其中，使得读者不止对理论基础能够有所了解，也能够明白最小二乘和李代数是如何被用来解决实际问题的。

一、基础理论知识

关于定位后端优化是如何从最大后验概率形式变成最小二乘问题，可以阅读笔者的两篇文章：《后端优化如何从最大后验概率到最小二乘（理论篇）/（应用篇）》

关于李代数基础知识可以阅读笔者这篇文章：《SLAM/定位中李代数基础概念及公式》（李代数基础知识也可以阅读SLAM十四讲的相应内容）

重要定理/性质备忘

PS:这部分直接跳过，后文推导过程中使用到这些性质再回头看即可

斜对称矩阵性质

\[ \begin{array}{lcl} \boldsymbol{a}^{\wedge}\boldsymbol{b} = -\boldsymbol{b}^{\wedge}\boldsymbol{a} && \forall \boldsymbol{a,b} \in \mathbb{R}^3 \end{array}\tag{1-1} \]

指数映射公式

\[ \exp(\boldsymbol{\phi}^{\wedge}) = \mathbf{I}+\frac{\sin{(||\boldsymbol{\phi}||)}}{||\boldsymbol{\phi}||}\boldsymbol{\phi}^{\wedge}+\frac{1-\cos{(||\boldsymbol{\phi}||)}}{||\boldsymbol{\phi}||^2}(\boldsymbol{\phi}^{\wedge})^2\tag{1-2} \]

指数映射一阶近似

\[ \exp(\boldsymbol{\phi}^{\wedge})\approx \mathbf{I}+\boldsymbol{\phi}^{\wedge}\tag{1-3} \]

向量形式的指数/对数映射

为了方便，我们记\(\operatorname{Exp}(\cdot)\)和\(\operatorname{Log}(\cdot)\)为向量形式的对数和指数映射

\[ \begin{array}{} \operatorname{Exp}:&\mathbb{R}^3 \rightarrow SO(3)\\ \operatorname{Log}:&SO(3)\rightarrow \mathbb{R}^3 \end{array}\tag{1-4} \]

BCH一阶近似

\[ \operatorname{Exp}(\phi+\delta{\phi})\approx \operatorname{Exp}(\phi)\operatorname{Exp}(J_r(\phi)\delta{\phi})\tag{1-5} \]

其中，\(J_r(\phi)\)称为右雅可比矩阵

BCH一阶近似还有另一种写法

\[ \operatorname{Log}\left(\operatorname{Exp}(\phi)\operatorname{Exp}(\delta{\phi})\right)\approx \phi + J_r^{-1}(\phi)\delta{\phi}\tag{1-6} \]

指数映射性质

\[ R\operatorname{Exp}(\phi)R^T=\operatorname{Exp}(R\phi^{\wedge}R^T)=\operatorname{Exp}(R\phi)\tag{1-7} \]

上式也可得

\[ \operatorname{Exp}(\phi)R = R\operatorname{Exp}(R^T\phi)\tag{1-8} \]

另有，当\(\delta{\phi}\)为微小量

\[ \operatorname{Exp}(-\delta{\phi})^T=\operatorname{Exp}(\delta{\phi})\tag{1-9} \]

右雅可比矩阵性质

在公式（1-5）中的\(J_r(\phi)\)，如果\(\phi\)是微小量，则其近似为单位矩阵。记为

\[ J_r(\delta{\phi})\approx\mathbf{I},\quad \text{when }\delta{\phi}\text{ is small}\tag{1-10} \]

旋转矩阵的扰动模型

当旋转矩阵的分布具有如下形式：

\[ \tilde{R}=R\operatorname{Exp}(\epsilon),\quad \epsilon \sim \mathcal{N}(0,\Sigma)\tag{1-11} \]

其负对数似然（negative log-likelihood）有如下形式

\[ \begin{split} \mathcal{L}(R)&=\frac{1}{2}||\operatorname{Log}(R^{-1}\tilde{R})||^2_{\Sigma}+const\\ &=\ \frac{1}{2}||\operatorname{Log}(\tilde{R}^{-1}R)||^2_{\Sigma}+const \end{split}\tag{1-12} \]

二、问题引出

目前SLAM方案包含前端和后端。前端使用视觉/激光，融合IMU数据输出一个里程计;后端尝试从历史轨迹和回环检测中优化出更准确的历史位姿。IMU在前后端都承担一个比较重要的角色。IMU数据进行积分，可以的到旋转和位置的估计，这个估计可以用来做视觉/点云匹配的初始位姿。IMU积分需要以某一帧的状态作为基础往下持续累加，但是SLAM后端的优化会改变历史轨迹位姿，导致IMU积分需要重新计算。为了避免重复积分的问题，可以将IMU的积分形式变化为对两个关键帧之间数据进行累积，获得两帧之间的相对状态变化，此称为IMU预积分

IMU预积分/积分

IMU积分 : 以某一时刻的状态为基础（位姿、速度），对后续的IMU输出（角速度、加速度）进行积分（累加），得到后续时刻的状态（位姿、速度）

IMU预积分 ：将两帧之间的IMU输出（角速度、加速度）进行积分（累加），得到两帧之间的相对状态（相对旋转、相对速度、相对位置）

坐标系

本文以视觉惯性SLAM为应用场景。IMU坐标系又称作Body坐标系，相机坐标系（Cam）与Body坐标系有一个固定位姿变换\(T_{BC}\)。车辆位姿为车辆在世界（World）坐标系下的姿态\(T_{WB}=(R_{WB},_W\boldsymbol{p})\)

推导的最终目的（重要！！）

推导最终目的是为了能够将预积分后的结果（状态State）构造成最小二乘形式，也就是误差的平方

为了能够获得最小二乘形式，需要将状态构造成真值+高斯噪声的形式，也就是测量值的似然函数，这样，只需要对似然函数取负对数即可。注意，还需要获得噪声的协方差表达，因为协方差的倒数，即信息矩阵，需要用来对二次项加权

\[ \begin{array}{lcl} \tilde{\mathcal{X}} = \mathcal{X} \oplus \epsilon, && \epsilon \sim \mathcal{N}\left(0,\Sigma\right)\tag{2-1} \end{array} \]

上述中\(\mathcal{X}\)表示状态，通常是旋转、平移、速度、IMU偏差等。上面公式之所以用复合符号\(\oplus\)是因为有些状态变量，比如旋转是不能直接相加的，对于旋转矩阵，\(\oplus\)是矩阵乘法

只要我们推导出公式（2-1），就可以将高斯分布用负对数的形式变化为最小二乘形式

\[ \underset{\mathcal{X}}{\operatorname{argmin}}||\tilde{\mathcal{X}}\ominus\mathcal{X}||^2_{\Sigma}\tag{2-2} \]

重申，我们需要推导出以下内容

状态向量的高斯分布形式（真值+高斯噪声）
噪声的协方差表达

方法要点（重要！！）

所有的推导都在流形（李群）上推导。本文的推导只用到了特殊正交群\(SO(3)\)。这样，优化过程可以在李代数空间上进行，不用考虑旋转矩阵的约束
为了避免优化的时候每一个IMU测量就需要增加一个优化因素，将两个关键帧之间的的所有IMU数据积分作为一个约束加入后端优化。（常见做法）
将IMU的积分公式重新推导成IMU预积分形式，使得当后端优化改变历史位姿时，不需要重新执行积分过程

推导过程总述

根据IMU模型，构造IMU积分形式（在流形上进行推导，使得优化可以在李代数空间上进行，不需要考虑旋转的约束）
将IMU积分公式变化为在两个关键帧之间所有IMU测量的积分
构造状态变化的预积分形式，使得积分过程在历史位姿由于后端优化变化之后不需要重新执行。此时还依赖历史时刻的IMU偏差
将噪声从预积分公式从预积分公式中剥离出来，构造（2-1）的标准形式
推导协方差的递推形式
将历史时刻的Bias从预积分公式中剥离，使得预积分公式完全不依赖历史时刻的状态
推导当IMU偏差更新的时候，如何更新预积分结果，避免重复计算

三、状态向量及优化目标

状态向量（State）描述\(i\)时刻车辆的旋转、位置、速度、IMU偏差

\[ \textbf{x}_i\overset{\cdot}{=}[\text{R}_i,\textbf{p}_i,\textbf{v}_i,\textbf{b}_i]\tag{3-1} \]

车辆（Body）坐标系与IMU坐标系假设为相同坐标系，因此上式也是\(i\)时刻的IMU状态

其中

\(\textbf{v}_i \in \mathbb{R}^3\)
\(\textbf{b}_i = [\textbf{b}_i^g \quad \textbf{b}_i^a] \in \mathbb{R}^6\)是角速度（gyroscope）和加速度（accelerometer）的偏差

记下列符号

\(\mathcal{K}_k\)为\(k\)时刻之前的所有关键帧
并记状态集合为

\[ \mathcal{X}_k \overset{\cdot}{=}\{\textbf{x}_i\}_{i\in \mathcal{K}_k}\tag{3-2} \]

\(\mathcal{I}_{ij},(i,j)\in \mathcal{K}_k\)为关键帧\(i,j\)之间的所有IMU测量

似然函数

\[ \begin{split} p(\mathcal{X}_k|\mathcal{I}_k) & \propto p(\mathcal{X}_0)p(\mathcal{I}_k|\mathcal{X}_k)\\ &=\ p(\mathcal{X}_0)\prod_{(i,j)\in \mathcal{K}_k}p(\mathcal{I}_{ij}|\textbf{x}_i, \textbf{x}_j) \end{split}\tag{3-3} \]

其中用到了独立同分布性质和马尔可夫假设

优化的最小二乘形式

\[ \begin{split} \mathcal{X}^{*}_k &=\ \underset{\mathcal{K}_k}{\operatorname{argmin}} -\log_ep(\mathcal{X}_k|\mathcal{I}_k)\\ &=\ \underset{\mathcal{K}_k}{\operatorname{argmin}}\sum_{(i,j)\in \mathcal{K}_k}||\mathbf{r}_{\mathcal{I}_{ij}}||^2_{\Sigma_{ij}} + ||\textbf{r}_0||^2_{\Sigma_0} \end{split}\tag{3-4} \]

四、IMU模型

\[ _\text{B}\tilde{\boldsymbol{\omega}}(t)= {_\text{B}\boldsymbol{\omega(t)}} + \textbf{b}^g(t)+ \boldsymbol{\eta}^g(t)\tag{4-1} \]

\[ _\text{B}\tilde{\textbf{a}}(t)=R^T_{\text{WB}}(t)\left({_\text{W}\textbf{a}(t)} - {_\text{W}\textbf{g}}\right) + \textbf{b}^a(t) + \boldsymbol{\eta}^a(t)\tag{4-2} \]

其中

\(_\text{B}\) 前下标意味着该量是在Body坐标系下。如 \(_\text{B}\tilde{\boldsymbol{\omega}}(t)\)是在Body坐标系下的角速度，也就是IMU陀螺仪的输出
\(\textbf{b}^g,\textbf{b}^a\)是IMU角速度和加速度的偏差，这两者也是一个随着时间缓慢变化的量
\(\boldsymbol{\eta}^g, \boldsymbol{\eta}^a\)是角速度和加速度的噪声。均值为0的高斯噪声

对于离散（discret）形式的高斯噪声\(\boldsymbol{\eta}^{gd}, \boldsymbol{\eta}^{ad}\)，他们的协方差与连续形式高斯噪声的功率谱强度具有如下关系

\[ \text{Cov}(\boldsymbol{\eta}^{gd}) = \frac{1}{\Delta{t}}\text{Cov}(\boldsymbol{\eta}^{g})\tag{4-3} \]

其中\(\Delta{t}\)是采样时间周期

五、运动模型及积分

为了使得整体思路清晰，简化推导过程，后文都会重点放在状态向量中的旋转部分

运动模型

\[ \begin{split} &\dot{R}_{\text{WB}}=R_{\text{WB}}\ {_{\text{B}}\boldsymbol{\omega}^{\wedge}}\\ \end{split}\tag{5-1} \]

上式中\(\dot{R}_{\text{WB}}\)表示\({R}_{\text{WB}}\)的导数。上式是旋转矩阵角运动方程，具体推导过程读者可以查找旋转矩阵求导相关资料

运动模型的积分

假设\(_{\text{B}}\boldsymbol{\omega}\)在时间间隔\([t,t+\Delta{t}]\)内保持不变，则公式（5-1）的离散形式积分 为

\[ R_{\text{WB}}(t+\Delta{t})=R_{\text{WB}}(t)\operatorname{Exp}\Big({_{\text{B}}\boldsymbol{\omega}}(t)\Delta{t}\Big)\tag{5-2} \]

公式（5-2）是微分方程（5-1）的解。如果这里思维卡住了，不要深究，记住（5-2）的结论即可

将IMU模型——公式（4-1）带入公式（5-1），可得

\[ R(t+\Delta{t})=R(t)\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}(t)-\textbf{b}^g(t)-\boldsymbol{\eta}^{gd}(t)\right)\Delta{t}\Big) \tag{5-3} \]

上式中\(\boldsymbol{\eta}^{gd}(t)\)是高斯噪声的离散形式，且协方差与连续形式的功率谱强度有公式（4-3）的联系

公式（5-3）去掉了上下标，不过现在应该不会引起歧义了

要注意的是 ，公式（5-3）是假设在IMU的采样周期\(\Delta{t}\)内角速度是不变的，这一点虽然不是事实，但是可以通过较高频率的IMU避免由于这一点引起的误差。（常见的IMU频率为200-500Hz）

运动模型积分的累积形式

公式（5-3）是对一个IMU的测量数据做积分，为了得到两个关键帧之间的积分，我们将关键帧\((i,j)\)之间的所有IMU测量都累计起来，可以得到如下形式

\[ R_j = R_i\prod^{j-1}_{k=i}\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_k-\boldsymbol{\eta}^{gd}_k\right)\Delta{t}\Big)\tag{5-4} \]

由于偏差是缓慢变化的（所谓的布朗运动，也就是白噪声的积分），所以在本文中，我们令关键帧\((i,j)\)之间的偏差保持不变

\[ \boldsymbol{\eta}^{gd}_i=\boldsymbol{\eta}^{gd}_{i+1}=...=\boldsymbol{\eta}^{gd}_{j-1}\tag{5-5} \]

六、预积分/相对增量形式

公式（5-4）让我们根据IMU的输出得到了两帧之间旋转的约束，但是公式（5-4）等号的右侧与\(i\)时刻的旋转和偏差相关，也就是与\(i\)时刻的状态 \(\textbf{x}_i\)相关。这里的缺憾在于，当我们在后端优化对历史轨迹优化之后，\(\textbf{x}_i\)会改变，这样公式（5-4）就需要重新计算。为了避免重复计算，我们将公式（5-4）进行调整为相对增量形式

\[ \Delta{R_{ij}}\overset{\cdot}{=}\prod^{j-1}_{k=i}\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_k-\boldsymbol{\eta}^{gd}_k\right)\Delta{t}\Big)\tag{6-1} \]

要注意的是 ，公式（6-1）依旧与\(i\)时刻的偏差\(\boldsymbol{\eta}^{gd}_i\)。我们暂时忽略这一点。在第九节，我们推导当偏差改变的时候，如何避免重复计算公式（6-1）

七、将噪声项独立构造高斯分布形式

公式（6-1）已经使用IMU的输出将\(i,j\)时刻的旋转联系起来。但是公式（6-1）之中将噪声项杂糅在各个累积项之中，这导致了我们无法简单地写出\(\Delta{R_{ij}}\)的概率分布函数。回顾第二节，我们的目标是构造标准的测量方程形式（2-1），这样我们才可以将MAP问题改写为最小二乘问题。为了达到这一个目的，我们进一步将噪声项独立出来

我们首先利用BCH一阶近似公式，即公式（1-5），将噪声项独立。公式（6-1）可以写成

\[ \Delta{R_{ij}}\overset{eq(1-5)}{\approx}\ \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_i\right) \Delta{t} \Big)\ \operatorname{Exp}\left(-J_r^k\boldsymbol{\eta}^{gd}_k\Delta{t}\right)\tag{7-1} \]

其中

\[ J_r^k \overset{\cdot}{=}J_r^k\left(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_i\right)\Delta{t}\right)\tag{7-2} \]

我们记

\[ \Delta{\tilde{R}_{ij}} \overset{\cdot}{=}\ \prod^{j-1}_{k=i}\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_i\right)\Delta{t}\Big)\tag{7-3} \]

观察公式（7-1），我们将累积中的最后两项抽出来

\[ \operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_{j \text{-} 2}-\textbf{b}^g_i\right) \Delta{t} \Big)\ \Bigg[\ \operatorname{Exp}\left(-J_r^{j \text{-} 2}\boldsymbol{\eta}^{gd}_{j \text{-} 2}\Delta{t}\right)\ \operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_{j \text{-} 1}-\textbf{b}^g_i\right) \Delta{t} \Big)\ \Bigg]\ \operatorname{Exp}\left(-J_r^{j \text{-} 1}\boldsymbol{\eta}^{gd}_{j \text{-} 1}\Delta{t}\right)\tag{7-3} \]

利用公式（1-8）\(\operatorname{Exp}(\phi)R = R\operatorname{Exp}(R^T\phi)\)，我们可以将中间两项改写如下

\[ \begin{split} &\operatorname{Exp}\left(-J_r^{j \text{-} 2}\boldsymbol{\eta}^{gd}_{j \text{-} 2}\Delta{t}\right)\ \operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_{j \text{-} 1}-\textbf{b}^g_i\right) \Delta{t} \Big)\\ =&\ \operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_{j \text{-} 1}-\textbf{b}^g_i\right) \Delta{t} \Big)\ \operatorname{Exp}\Big(-\Delta{\tilde{R}^T_{j\text{-}1j}}J_r^{j \text{-} 2}\boldsymbol{\eta}^{gd}_{j \text{-} 2}\Delta{t}\Big) \end{split}\tag{7-4} \]

对公式（7-1）从后往前应用公式（7-4），我们可以将噪声项独立出来

\[ \begin{split} \Delta{R_{ij}}\ &=\ \Delta{\tilde{R}_{ij}}\ \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big(-\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\Big)\\ &\overset{\cdot}{=}\ \Delta{\tilde{R}_{ij}}\ \operatorname{Exp}(-\delta\phi_{ij}) \end{split}\tag{7-5} \]

于是，我们可以将\(\Delta{\tilde{R}_{ij}}\)写为如公式（2-1）的标准形式，回顾公式（1-9）\(\operatorname{Exp}(-\delta{\phi})^T=\operatorname{Exp}(\delta{\phi})\)

\[ \begin{split} \Delta{\tilde{R}_{ij}}\ =\ \Delta{R_{ij}}\ \operatorname{Exp}(\delta\phi_{ij})\ \end{split}\tag{7-6} \]

八、协方差矩阵的递归形式

公式（7-6）推导出了如公式（2-1）的标准形式，但是还差一点，就是噪声项\(\operatorname{Exp}(-\delta\phi_{ij})\)是否符合0均值的高斯分布，以及它的协方差是怎么样的。

这一节的目的是推导噪声项的分布以及协方差的增量形式

让我们从公式（7-5）摘录噪声项

\[ \operatorname{Exp}(-\delta\phi_{ij})\ =\ \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big(-\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\Big)\tag{8-1} \]

记

\[ \xi_k=\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\tag{8-2} \]

由于\(\boldsymbol{\eta}^{gd}_{k}\)是个极微小量，而且\(\delta{\phi}_{ij}\)也是个微小量，所以\(\xi_k\)整体也是微小量，因此，根据性质（1-9）

\[ J_r(\xi_k)\approx \mathbf{I}\tag{8-3} \]

因此，我们根据公式（1-6），将（8-1）可以写成

\[ \operatorname{Exp}(-\delta\phi_{ij})\ =\ \operatorname{Exp}\left(-\sum^{j-1}_{k=i}\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\right)\tag{8-4} \]

即

\[ \delta\phi_{ij}\ \approx\ \sum^{j-1}_{k=i}\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\tag{8-5} \]

从公式（8-5）我们可以看到，公式（7-6）的噪声项是高斯噪声\(\boldsymbol{\eta}^{gd}\)的线性组合，因此其本身也是高斯噪声

我们接下来进一步推导这个噪声的协方差的表达形式

\[ \begin{split} \delta\phi_{ij}\ &\approx\ \sum^{j-1}_{k=i}\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\\ &=\ \sum^{j-1}_{k=i}\Delta{\tilde{R}^T_{k+1,j}}J_r^k\boldsymbol{\eta}^{gd}_k\Delta{t}\ +\ \overset{=\mathbf{I}_{3\times 3}}{\overbrace{\Delta{\tilde{R}^T_{j,j}}}}\ J_r^{j-1}\boldsymbol{\eta}^{gd}_{j-1}\Delta{t}\\ &=\ \sum^{j-1}_{k=i}(\overset{=\Delta{\tilde{R}_{k+1,j}}}{\overbrace{\Delta{\tilde{R}_{k+1,j-1}}\Delta{\tilde{R}_{j-1,j}}}})\ +\ J_r^{j-1}\boldsymbol{\eta}^{gd}_{j-1}\Delta{t}\\ &=\ \Delta{\tilde{R}^T_{j-1,j}}\sum^{j-2}_{k=i}\Delta{\tilde{R}^T_{k+1,j-1}}J_r^k\boldsymbol{\eta}^{gd}_k\Delta{t}\ +\ J_r^{j-1}\boldsymbol{\eta}^{gd}_{j-1}\Delta{t}\\ &=\ \Delta{\tilde{R}^T_{j-1,j}}\delta{\phi}_{ij-1}+J_r^{j-1}\boldsymbol{\eta}^{gd}_{j-1}\Delta{t} \end{split}\tag{8-6} \]

根据高斯分布协方差的性质

\[ \Sigma_{ij}\ =\ \Delta{\tilde{R}^T_{j-1,j}}\ \Sigma_{ij-1}\ \Delta{\tilde{R}_{j-1,j}}\ +\ (J_r^{j-1}\Delta{t})\ \Sigma_{\boldsymbol{\eta}}\ (J_r^{j-1}\Delta{t})^T\tag{8-7} \]

同时我们可以令协方差的初始值\(\Sigma_{ii}=\textbf{0}_{9\times 9}\)

于是，我们就得出了协方差的递归形式

九、偏差项Bias更新

暂时性总结

到目前位置，我们已经推导出了如下内容

测量方程的标准表达形式，即公式（7-6）
测量方程噪声协方差的递归表达形式，即公式（8-7）

但是要注意，公式（7-6）里面还偏差项\(\textbf{b}_i^g\)。也就是其依旧依赖\(i\)时刻的系统状态。那么如何在偏差更新的时候，避免测量方程的重复计算？这就是本节要推导的内容

纳入偏差更新

当偏差项更新

\[ \hat{\textbf{b}} \leftarrow \bar{\textbf{b}}+\delta{\textbf{b}}\tag{9-1} \]

我们希望计算更新后的测量\(\Delta{\tilde{R}_{ij}(\hat{\textbf{b}}^g_i)}\)，相比与原先的值\(\Delta{\tilde{R}_{ij}(\bar{\textbf{b}}^g_i)}\)的更新量

这样我们就不需要重复计算\(\Delta{\tilde{R}_{ij}}\)

把\(\hat{\textbf{b}}^g_i\)代入\(\Delta{\tilde{R}_{ij}}\)的表达式（7-3）

\[ \Delta{\tilde{R}}_{ij}(\hat{\textbf{b}}^g_i)\ =\ \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big( \left( \tilde{\boldsymbol{\omega}}_k-\hat{\textbf{b}}^g_i \right)\Delta{t} \Big)\tag{9-2} \]

将公式（9-1）代入（9-2），并利用BCH一阶近似公式（1-5）展开

\[ \begin{split} \Delta{\tilde{R}}_{ij}(\hat{\textbf{b}}^g_i)\ & = \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big( \left( \tilde{\boldsymbol{\omega}}_k-(\bar{\textbf{b}}^g_i+\delta{\textbf{b}}^g_i) \right)\Delta{t} \Big)\\ & = \prod^{j-1}_{k=i}\ \operatorname{Exp}\Big( \left(\tilde{\boldsymbol{\omega}}_k-\bar{\textbf{b}}^g_i\right)\Delta{t} \Big)\ \operatorname{Exp}\Big( -J_r^k\delta{\textbf{b}}^g_i\Delta{t} \Big) \end{split}\tag{9-3} \]

采用推导公式（7-1）到（7-5）的方式，从后往前推，并利用公式（1-8），上式可以写为

\[ \begin{split} \Delta{\tilde{R}}_{ij}(\hat{\textbf{b}}^g_i)\ =\Delta{\bar{R}_{ij}}\prod^{j-1}_{k=i}\ \operatorname{Exp}\Big( -\Delta{\bar{R}}_{k+1,j}^TJ_r^k\delta{\textbf{b}^g_i}\Delta{t} \Big) \end{split}\tag{9-4} \]

我们再采用类似（8-1）到（8-5）的推导方法，（\(\delta{\textbf{b}}\)是一个极小量，所以展开时的右雅可比接近单位矩阵）

公式（9-4）可以写为

\[ \begin{split} \Delta{\tilde{R}}_{ij}(\hat{\textbf{b}}^g_i)\ & \approx \Delta{\bar{R}_{ij}}\ \operatorname{Exp}\Big( \sum^{j-1}_{k=i}-\Delta{\bar{R}}_{k+1,j}^TJ_r^k\delta{\textbf{b}^g_i}\Delta{t} \Big)\\ & \overset{\cdot}{=} \Delta{\bar{R}_{ij}}\ \operatorname{Exp}\Big( \frac{\partial{\Delta{\bar{R}^{\vee}_{ij}}}}{\partial{\textbf{b}^g}}\ \delta{\textbf{b}^g_i} \Big) \end{split}\tag{9-5} \]

十、最小二乘形式/误差

从公式（1-12）我们提前构造好了旋转的最大似然的负对数形式

\[ \begin{split} \mathcal{L}(R) & = \frac{1}{2}||\operatorname{Log}(\tilde{R}^{-1}R)||^2_{\Sigma}+const \end{split} \]

我们已经构造了相对旋转的表达形式（9-5），因此我们可以进一步得出了旋转的误差模型

\[ \begin{split} \textbf{r}_{\Delta{R}_{ij}} = \operatorname{Log}\left( \bigg( \Delta{\bar{R}_{ij}}\ \operatorname{Exp}\big( \frac{\partial{\Delta{\bar{R}^{\vee}_{ij}}}}{\partial{\textbf{b}^g}}\ \delta{\textbf{b}^g_i} \big) \bigg)^T R_i^TR_j \right) \end{split}\tag{10-1} \]

总结

本文主要从论文《On-Manifold Preintegration for Real-TimeVisual-Inertial Odometry》出发，挑选其中的旋转矩阵相关内容，目的是为了能够从实际应用出发，联系后端优化的非线性最小二乘优化问题和李群李代数知识，加深理论知识的印象。

再次强调，本文需要拥有一定的李群李代数基础知识，以及后端优化中如何将最大后验概率问题转化到最小二乘问题，没有这两个部分的知识可以参考笔者整理的两篇笔记。

本文只挑选了系统状态中的旋转分量相关的内容。实际上原论文对位置、速度的推导也十分详细。但是太过杂乱的内容不仅对理解整个推导过程思路是如何来的没有帮助，还会让读者一头雾水。

本文首先给出一些基础知识和后续推导用的性质（第一节）

第二节的重点是让读者要明白这么长篇大论的推导过程的线索是什么，最终目的是什么？我们的最终目的是将测量与真实值能够写成如公式（2-1）的标准形式，这样可以直接根据高斯分布的性质将最大似然问题通过负对数转化为最小二乘问题。

\[ \begin{array}{lcl} \tilde{\mathcal{X}} = \mathcal{X} \oplus \epsilon, && \epsilon \sim \mathcal{N}\left(0,\Sigma\right)\tag{2-1} \end{array} \]

第三节到第五节，我们推导了如何根据IMU的输出数据，将关键帧\((i,j)\)的位姿中的旋转分量联系起来，即通过公式（5-4）

\[ R_j = R_i\prod^{j-1}_{k=i}\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_k-\boldsymbol{\eta}^{gd}_k\right)\Delta{t}\Big)\tag{5-4} \]

为了避免优化过后关键帧\(i\)的状态改变，积分需要重复计算的问题，我们在第六节推导了IMU预积分形式，也就是两帧之间的相对运动形式，即公式（6-1）

\[ \Delta{R_{ij}}\overset{\cdot}{=}\prod^{j-1}_{k=i}\operatorname{Exp}\Big(\left(\tilde{\boldsymbol{\omega}}_k-\textbf{b}^g_k-\boldsymbol{\eta}^{gd}_k\right)\Delta{t}\Big)\tag{6-1} \]

为了构造标准形式（2-1），我们在第七节中将噪声项独立出来，成功构造标准形式，即公式（7-6）

\[ \begin{split} \Delta{\tilde{R}_{ij}}\ =\ \Delta{R_{ij}}\ \operatorname{Exp}(\delta\phi_{ij})\ \end{split}\tag{7-6} \]

为了在最小二乘问题中使用噪声的协方差矩阵加权，我们在第八节推导了噪声的分布及方差的递归形式，即公式（8-5）

\[ \delta\phi_{ij}\ \approx\ \sum^{j-1}_{k=i}\Delta{\tilde{R}^T_{k\text{+}1j}}J_r^{k}\boldsymbol{\eta}^{gd}_{k}\Delta{t}\tag{8-5} \]

最后，我们来解决如何在IMU偏差更新的时候，避免积分过程需要重复计算的问题，即公式（9-5）

剩下的问题也就自然解决，我们可以根据标准形式构造误差函数的表达，即公式（10-1）