李群流形性质
hat(\(\bullet ^ \wedge\)) vee(\(\bullet ^\vee\))
\[
\mathbf{\omega}^{\wedge}=\begin{bmatrix}\omega_1\\ \omega_2\\ \omega_3\end{bmatrix}^{\wedge}=\begin{bmatrix}0&-\omega_3&\omega_2\\ \omega_3&0&-\omega_1\\ -\omega_2&\omega_1&0\end{bmatrix}=\mathbf{W} \\
\mathbf{W}^{\vee}=\begin{bmatrix}0&-\omega_3&\omega_2\\ \omega_3&0&-\omega_1\\ -\omega_2&\omega_1& 0 \end{bmatrix}^{\vee}=\begin{bmatrix}\omega_1\\ \omega_2\\ \omega_3\end{bmatrix}=\mathbf{\omega}
\]
hat 运算性质：
\[
\textbf{a}^{\wedge}\cdot\textbf{b}=-\textbf{b}^{\wedge}\cdot\textbf{a},\quad\forall\textbf{a},\textbf{b}\in R^3
\]
指数映射\(\text{exp}\left(\bullet\right)\)将李代数映射到李群：
\[
\exp\left(\vec{\phi}^{\wedge}\right)=\mathbf{I}+\dfrac{\sin\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|}\vec{\phi}^{\wedge}+\dfrac{1-\cos\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^2}\left(\vec{\phi}^{\wedge}\right)^2
\]
当\(\vec{\phi}\)是小量时,有一阶近似：
\[
\text{}\exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}+\vec{\phi}^{\wedge}
\]
对数映射\(\log\left(\bullet\right)\)将李群映射到李代数：
\[
\begin{aligned}\log\left(\textbf{R}\right)&=\frac{\varphi\cdot\left(\textbf{R}-\textbf{R}^T\right)}{2\sin\left(\varphi\right)}\end{aligned} \\
\varphi=\cos^{-1}\left(\dfrac{tr\big(\mathbf{R}\big)-1}{2}\right)
\]
得：
\[
\begin{aligned}\log\left(\textbf{R}\right)^\vee=\varphi\cdot\textbf{a}\end{aligned} \\
\quad\textbf{a}=\left(\dfrac{\textbf{R}-\textbf{R}^T}{2\sin\left(\varphi\right)}\right)^\vee
\]
其中：\(\varphi\)为旋转角,\(\textbf{a}\)为旋转轴单位矢量.当\(\|\vec{\phi}\|<\pi\)指数映射和对数映射构成双射。
定义符号，表示李群（\(3\times 3\)正交矩阵）和李代数（\(3\times 3\)反对称矩阵）之间的映射：
\[
\begin{aligned}&\text{Exp}:R^3\ni\vec{\phi}\to\exp\left(\vec{\phi}^{\wedge}\right)\in SO(3)\\ &\text{Log}:S O(3)\ni\mathbf{R}\to\log\left(\mathbf{R}\right)^{\vee}\in R^3\end{aligned}
\]
对于3维实向量\(\vec{\phi}\)和一个小量\(\delta\vec{\phi}\),有下述近似性质:
\[
\begin{aligned}\text{Exp}\left(\vec{\phi}+\delta\vec{\phi}\right)\approx\text{Exp}\left(\vec{\phi}\right)\cdot\text{Exp}\left(\mathbf{J}_r\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\right)\\ \text{Log}\left(\text{Exp}\left(\vec{\phi}\right)\cdot\text{Exp}\left(\delta\vec{\phi}\right)\right)=\vec{\phi}+\mathbf{J}_r^{-1}\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\end{aligned}
\]
理解为:
\(\mathbf{R}^3\)中的向量\(\vec{\phi}\)加上一个小量\(\delta\vec{\phi}\)，对应到\(SO(3)\)中则\(Exp(\vec{\phi})\)是右乘一个\(\mathrm{Exp}\left(\mathbf{J}_{r}\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\right)\)。
\(SO(3)\)中\(Exp(\vec{\phi})\)右乘一个\(Exp(\delta\vec{\phi})\)，对应到\(\mathbf{R}^3\)中则是\(\vec{\phi}\)加上一项\(\textbf{J}_r^{-1}\Big(\vec{\phi}\Big)\cdot\delta\vec{\phi}\)。
\(\textbf{J}_r \Big(\vec{\phi}\Big)\)是\(SO(3)\)的右Jacobian，将切空间的“加性项”和\(SO(3)\)“乘性项”联系在了一起。
\[
\mathbf{J}_{r}\left(\vec{\phi}\right)=\mathbf{I}-\frac{1-\cos\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^{2}}\vec{\phi}^{\wedge}+\frac{\left\|\vec{\phi}\right\|-\sin\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^{3}}\left(\vec{\phi}^{\wedge}\right)^{2} \\
\mathbf{J}_r^{-1}\left(\vec{\phi}\right)=\mathbf{I}+\dfrac{1}{2}\vec{\phi}^{\wedge}+\left(\dfrac{1}{\left\|\vec{\phi}\right\|^2}-\dfrac{1+\cos\left(\left\|\vec{\phi}\right\|\right)}{2\cdot\left\|\vec{\phi}\right\|\cdot\sin\left(\left\|\phi\right\|\right)}\right)\left(\vec{\phi}^{\wedge}\right)^2
\]
指数映射Adjoint：
\[
\begin{array}{l}\mathbf{R}\cdot\mathrm{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}^T=\mathrm{exp}\left(\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T\right)=\mathrm{Exp}\left(\mathbf{R}\vec{\phi}\right)\\ \Leftrightarrow\mathrm{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}=\mathbf{R}\cdot\mathrm{Exp}\left(\mathbf{R}^T\vec{\phi}\right)\end{array}
\]
证明： 对于任意旋转矩阵\(\mathbf{R}\)和三维矢量\(\vec{\phi}\), 有如下恒等式：
\[
\left(\mathbf{R}\vec{\phi}\right)^{\wedge}=\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T
\]
由指数映射（罗德里格斯公式）：
\[
\begin{array}{l}\text{Exp}\left(\vec{\phi}\right)=\exp\left(\vec{\phi}\wedge\right)=\exp\left(\theta\mathbf{a}^\wedge\right)\\ =\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\mathbf{a}\mathbf{a}^T+\sin\theta\cdot\mathbf{a}^\wedge\end{array}
\]
其中\(\vec{\phi} = \theta \mathbf{a}\),为\(\theta\)模值,\(\mathbf{a}\)为单位矢量。
变形如下：
\[
\begin{align}
&\mathbf{R}\cdot\text{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}^T\\ &=\mathbf{R}\cdot\left[\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\mathbf{aa}^T+\sin\theta\cdot\mathbf{a}^\wedge\right]\cdot\mathbf{R}^T\\
&=\cos\theta\cdot\mathbf{R}\mathbf{R}^T+\left(1-\cos\theta\right)\cdot\mathbf{R}\mathbf{a}\mathbf{a}^T \mathbf{R}^T+\sin\theta\cdot\mathbf{R}\mathbf{a}^\wedge\mathbf{R}^T\\
&=\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\cdot\left(\mathbf{R}\mathbf{a}\right)\cdot\left(\mathbf{R}\mathbf{a}\right)^T+\sin\theta\cdot\left(\mathbf{R}\mathbf{a}\right)^{\wedge}\\ &=\exp\left(\theta\left(\mathbf{R}\mathbf{a}\right)^{\wedge}\right)=\exp\left(\left(\mathbf{R}\vec{\phi}\right)^{\wedge}\right) \\
&=\exp\left(\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T\right) \\
&= \text{Exp}\left(\mathbf{R} \vec{\phi}\right)
\end{align}
\]
IMU 运动学
陀螺仪与加速度计测量模型：
\[
\begin{align}
&\tilde{\mathbf{\omega}}_{wb}^b\left(t\right)=\mathbf{\omega}_{wb}^b\left(t\right)+\mathbf{b}_g\left(t\right)+\mathbf{\eta}_g\left(t\right) \\
&\mathbf{f}^b\left(t\right)=\mathbf{R}_b^{wT}\left(\mathbf{a}^w-\mathbf{g}^w\right)+\mathbf{b}_a\left(t\right)+\mathbf{\eta}_a\left(t\right)
\end{align}
\]
其中\(\mathbf{b}_g,\mathbf{b}_a\)随时间缓慢变化的bias。\(\mathbf{\eta}_g,\mathbf{\eta}_a\)是白噪声。
运动模型的微分方程:
\[
\begin{aligned} \dot{\mathbf{R}} &= \mathbf{R} \boldsymbol{\omega}^\wedge, \\ \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{v}} &= \mathbf{a} \end{aligned}
\]
进行欧拉积分得：
\[
\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t) \mathrm{Exp} (\boldsymbol{\omega}(t) \Delta t) \\ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{a}(t) \Delta t \\ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t) \Delta t + \frac{1}{2} \mathbf{a}(t) \Delta t^2 \end{align}
\]
其中角速度和加速度可以被IMU测量到，但受到噪声与重力影响。令测量值为\(\tilde{\boldsymbol{\omega}}\)和\(\tilde{\mathbf{a}}\)：
\[
\begin{align} \tilde{\boldsymbol{\omega}}(t) &= \boldsymbol{\omega}(t) + \mathbf{b}_g (t) + \boldsymbol{\eta}_g (t) \\ \tilde{\mathbf{a}}(t) &= \mathbf{R}^\mathrm{T} (\mathbf{a}(t) - \mathbf{g}) + \mathbf{b}_a (t) + \boldsymbol{\eta}_a (t) \end{align}
\]
其中\(\mathbf{b}_g\),\(\mathbf{b}_a\)为陀螺和加速度计零偏，\(\boldsymbol{\eta}_a\),\(\boldsymbol{\eta}_g\)为测量的高斯噪声。把该式代入上式，可得到测量值与状态变量的关系：
\[
\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t) \mathrm{Exp} \left((\tilde{\boldsymbol{\omega}} - \mathbf{b}_g(t) - \boldsymbol{\eta}_{gd} (t))\Delta t \right) \\ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g} \Delta t + \mathbf{R}(t) (\tilde{\mathbf{a}} -\mathbf{b}_a(t) - \boldsymbol{\eta}_{ad}(t)) \Delta t \\ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t) \Delta t + \frac{1}{2}\mathbf{g} \Delta t^2 + \frac{1}{2} \mathbf{R}(t) (\tilde{\mathbf{a}} -\mathbf{b}_a(t) - \boldsymbol{\eta}_{ad}(t)) \Delta t^2 \end{align}
\]
其中\(\boldsymbol{\eta}_{gd}\),\(\boldsymbol{\eta}_{ad}\)是离散化后的随机游走噪声[6]:
\[
\begin{align} \mathrm{Cov}(\boldsymbol{\eta}_{gd}(t) ) &= \frac{1}{\Delta t} \mathrm{Cov} (\boldsymbol{\eta}_g(t)) \\ \mathrm{Cov}(\boldsymbol{\eta}_{ad}(t) ) &= \frac{1}{\Delta t} \mathrm{Cov} (\boldsymbol{\eta}_a(t)) \end{align}
\]
预积分
在优化算法中，为了避免状态量(\(\mathbf{R} , \mathbf{p}, \mathbf{v}\))迭代变化导致需要重新计算积分，考虑将一段时间IMU打包，称为预积分。
\[
\begin{align} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=i}^{j-1} (\mathrm{Exp} \left(\left( \tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,k} - \boldsymbol{\eta}_{gd, k} \right) \Delta t \right)\\ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g} \Delta t_{ij} + \sum_{k=i}^{j-1} \mathbf{R}_k (\tilde{\mathbf{a}} - \mathbf{b}_{a, k} - \boldsymbol{\eta}_{ad, k}) \Delta t \\ \mathbf{p}_j &= \mathbf{p}_i + \sum_{k=i}^{j-1} \mathbf{v}_k \Delta t + \frac{1}{2} \mathbf{g} \Delta t_{ij}^2 + \frac{1}{2} \sum_{k=i}^{j-1} \mathbf{R}_k (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k}) \Delta t^2 \end{align}
\]
其中\(\Delta t_{ij} = \sum_{k=i}^{j-1} \Delta t\)，为累计时间。 将上式变换，定义相对变化量：
\[
\begin{align}
\Delta \mathbf{R}_{ij} &= \mathbf{R}_i^\mathrm{T} \mathbf{R}_j = \prod_{k=i}^{j-1} \mathrm{Exp} \left( \left(\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,k} - \boldsymbol{\eta}_{gd, k}\right) \Delta t\right) \\
\Delta \mathbf{v}_{ij} &= \mathbf{R}_i^\mathrm{T}(\mathbf{v}_j - \mathbf{v}_i -\mathbf{g} \Delta t_{ij}) = \sum_{k=i}^{j-1} \Delta \mathbf{R}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k}) \Delta t\\ \Delta \mathbf{p}_{ij} &= \mathbf{R}_i^\mathrm{T} \left( \mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac{1}{2} \mathbf{g} \Delta t_{ij}^2 \right) \\
&= \sum_{k=i}^{j-1} \left[\Delta \mathbf{v}_{ik} \Delta t+\frac{1}{2} \Delta \mathbf{R}_{ik} \left(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k} \right)\Delta t^2 \right]
\end{align}
\]
预积分噪声模型
固定\(i\)时刻的零偏估计,把噪声项分离出去，来分析预积分的噪声。从而定义预积分测量\(\Delta \tilde{\mathbf{R}}_{ij}, \Delta \tilde{\mathbf{v}}_{ij}, \Delta \tilde{\mathbf{p}}_{ij}\)。
对于旋转：
\[
\begin{aligned} \Delta \mathbf{R}_{ij} &= \prod_{k=i}^{j-1} \mathrm{Exp} \left( \left(\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,k} - \boldsymbol{\eta}_{gd, k}\right) \Delta t\right) \\ &\approx \prod_{k=i}^{j-1} \left[ \mathrm{Exp}\left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{i,g}) \Delta t \right) \mathrm{Exp} \left(-\mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t \right) \right] \\ \end{aligned}
\]
定义测量量：
\[
\Delta \tilde{\mathbf{R}}_{ij} = \prod_{k=i}^{j-1} \mathrm{Exp}\left( (\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,i})\Delta t \right)
\]
将预积分公式展开，不断地把观测置换到左侧，并把噪声置换到右侧，并且把噪声项内部的\(\Delta \tilde{\mathbf{R}}\)项合并：
\[
\begin{aligned} \Delta \mathbf{R}_{ij} &= \underbrace{\mathrm{Exp}\left((\tilde{\boldsymbol{\omega}}_i - \mathbf{b}_{i,g}) \Delta t \right)}_{\Delta\tilde{\mathbf{R}}_{i, i+1}} \mathrm{Exp} \left(-\mathbf{J}_{r,i} \boldsymbol{\eta}_{gd, i} \Delta t \right) \underbrace{\mathrm{Exp}\left((\tilde{\boldsymbol{\omega}}_{i+1} - \mathbf{b}_{i,g}) \Delta t \right)}_{\Delta \tilde{\mathbf{R}}_{i+1, i+2}}\mathrm{Exp} \left(-\mathbf{J}_{r,i+1} \boldsymbol{\eta}_{gd, i} \Delta t \right) \ldots \\ &= \Delta \tilde{\mathbf{R}}_{i, i+1} \underbrace{\mathrm{Exp} \left(-\mathbf{J}_{r,i} \boldsymbol{\eta}_{gd, i} \Delta t \right)\Delta\tilde{\mathbf{R}}_{i+1, i+2}}_{=\Delta \tilde{\mathbf{R}}_{i+1, i+2} \mathrm{Exp}(-\Delta \tilde{\mathbf{R}}_{i+1, i+2}^\mathrm{T} \mathbf{J}_{r,i} \boldsymbol{\eta}_{gd, i} \Delta t)} \mathrm{Exp} \left(-\mathbf{J}_{r,i+1} \boldsymbol{\eta}_{gd, i} \Delta t \right) \ldots \\ &=\Delta \tilde{\mathbf{R}}_{i, i+2} \mathrm{Exp}(-\Delta \tilde{\mathbf{R}}_{i+1, i+2}^\mathrm{T} \mathbf{J}_{r,i} \boldsymbol{\eta}_{gd, i} \Delta t) \mathrm{Exp} \left(-\mathbf{J}_{r,i+1} \boldsymbol{\eta}_{gd, i} \Delta t \right) \Delta \tilde{\mathbf{R}}_{i+2, i+3} \ldots \end{aligned}
\]
\[
\begin{aligned} \Delta \mathbf{R}_{ij} &= \Delta \tilde{\mathbf{R}}_{ij} \prod_{k=i}^{j-1} \mathrm{Exp}\left( -\Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t \right) \\ & = \Delta \tilde{\mathbf{R}}_{ij} \mathrm{Exp} (-\delta \boldsymbol{\phi}_{ij}) \end{aligned}
\]
对于速度，带入旋转测量量\(\Delta \tilde{\mathbf{R}}_{ij}\)：
\[
\begin{aligned} \Delta \mathbf{v}_{ij} &= \sum_{k=i}^{j-1} \Delta \mathbf{R}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k}) \Delta t\\ &= \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \underbrace{\mathrm{Exp} (-\delta \boldsymbol{\phi}_{ik})}_{\approx \mathbf{I} - \delta \boldsymbol{\phi}_{ik}^\wedge } (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k}) \Delta t \\ &= \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} (\mathbf{I} - \delta \boldsymbol{\phi}^\wedge_{ik})(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k}) \Delta t \end{aligned}
\]
我们舍掉上式中的噪声二阶小量，定义测量量：
\[
\Delta \tilde{\mathbf{v}}_{ij} = \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i}) \Delta t
\]
\[
\begin{aligned} \Delta \mathbf{v}_{ij} &= \sum_{k=i}^{j-1} \underbrace{\Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i}) \Delta t}_{\text{累加此项}} + \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t - \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad, k} \Delta t \\ &= \Delta \tilde{\mathbf{v}}_{ij} + \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t - \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad, k} \Delta t \\ &= \Delta \tilde{\mathbf{v}}_{ij} - \delta \mathbf{v}_{ij} \end{aligned}
\]
对于位置，带入旋转和速度测量量：
\[
\begin{aligned} \Delta \mathbf{p}_{ij} &= \sum_{k=i}^{j-1} \left[\Delta \mathbf{v}_{ik} \Delta t+\frac{1}{2} \Delta \mathbf{R}_{ik} \left(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k} \right)\Delta t^2 \right] \\ &= \sum_{k=i}^{j-1} \left[(\Delta \tilde{\mathbf{v}}_{ij} - \delta \mathbf{v}_{ij}) \Delta t + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \underbrace{\mathrm{Exp}(-\delta \boldsymbol{\phi}_{ik})}_{\mathbf{I} - \delta \boldsymbol{\phi}_{ij}^\wedge} \left(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,k} - \boldsymbol{\eta}_{ad, k} \right)\Delta t^2 \right] \\ &\approx \sum_{k=i}^{j-1} \left[ (\Delta \tilde{\mathbf{v}}_{ik} - \delta \mathbf{v}_{ik}) \Delta t + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} (\mathbf{I} - \delta \boldsymbol{\phi}^\wedge_{ik}) (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i}) \Delta t^2 - \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad, k} \Delta t^2 ) \right] \\ &\approx \sum_{k=i}^{j-1} \left[ \Delta \tilde{\mathbf{v}}_{ik} \Delta t + \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i}) \Delta t^2 -\delta \mathbf{v}_{ik} \Delta t + \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t^2 - \right. \\ & \quad \quad \quad \left. \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad, k} \Delta t^2 \right] \end{aligned}
\]
在第三式至第四式中我们舍去了二阶噪声小量。定义测量量：
\[
\begin{equation} \Delta \tilde{\mathbf{p}}_{ij} = \sum_{k=i}^{j-1} \left[ (\Delta \tilde{\mathbf{v}}_{ik} \Delta t) + \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i}) \Delta t^2 \right] \end{equation}
\]
\[
\begin{aligned} \Delta \mathbf{p}_{ij} &= \Delta \tilde{\mathbf{p}}_{ij} + \sum_{k=i}^{j-1} \left[-\delta \mathbf{v}_{ik} \Delta t + \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t^2 - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad, k} \Delta t^2 \right] \\ & = \Delta\tilde{\mathbf{p}}_{ij} - \delta \mathbf{p}_{ij} \end{aligned}
\]
至此，得到预积分测量量以及噪声：
\[
\begin{align} \Delta \tilde{\mathbf{R}}_{ij} &= \mathbf{R}_i^\mathrm{T} \mathbf{R}_j \mathrm{Exp}(\delta \boldsymbol{\phi}_{ij}) \\ \Delta \tilde{\mathbf{v}}_{ij} &= \mathbf{R}_i^\mathrm{T} \left( \mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij} \right) + \delta \mathbf{v}_{ij} \\ \Delta \tilde{\mathbf{p}}_{ij} &= \mathbf{R}_i^\mathrm{T} \left(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac{1}{2}\mathbf{g} \Delta t_{ij}^2 \right) + \delta \mathbf{p}_{ij} \end{align}
\]
对于旋转噪声量：
\[
\mathrm{Exp}(\delta \boldsymbol{\phi}_{ij}) = \prod_{k=i}^{j-1} \mathrm{Exp}(- \Delta \tilde{\mathbf{R}}_{k+1}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t)
\]
对两侧取\(\mathbf{Log}\)，可得：
\[
\delta \boldsymbol{\phi}_{ij} = -\mathrm{Log} \left( \prod_{k=i}^{j-1} \mathrm{Exp}(- \Delta \tilde{\mathbf{R}}_{k+1}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t) \right)
\]
通过BCH进行线性近似。同时，由于内部的系数项\(- \Delta \tilde{\mathbf{R}}_{k+1}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t\)已经为噪声，接近于0，我们可以将BCH近似的右雅可比取为单位阵\(\mathbf{I}\)，那么可以得到：
\[
\delta \boldsymbol{\phi}_{ij} \approx \sum_{k=i}^{j-1} \Delta \mathbf{R}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd,k} \Delta t
\]
此式是高斯随机变量的线性组合，它的结果依然是高斯的。同时，由于预积分的累加特性，预测分观测量的噪声也会随着时间不断累加。用第\(j-1\)时刻的噪声来计算第\(j\)时刻的噪声。
\[
\begin{aligned} \delta \boldsymbol{\phi}_{ij} &\approx \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t\\ & = \sum_{k=i}^{j-2} \Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t + \underbrace{\Delta \mathbf{R}_{j,j}^\mathrm{T}}_{=\mathbf{I}} \mathbf{J}_{r, j-1} \boldsymbol{\eta}_{gd, j-1} \Delta t\\ &= \sum_{k=i}^{j-2} \underbrace{\Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T}}_{\left(\Delta \tilde{\mathbf{R}}_{k+1, j-1} \Delta \tilde{\mathbf{R}}_{j-1, j}\right)^\mathrm{T}} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t + \mathbf{J}_{r, j-1} \boldsymbol{\eta}_{gd, j-1} \Delta t\\ &= \Delta \tilde{\mathbf{R}}_{j-1, j}^\mathrm{T} \sum_{k=i}^{j-2} \Delta \tilde{\mathbf{R}}_{k+1, j-1}^\mathrm{T} \mathbf{J}_{r,k} \boldsymbol{\eta}_{gd, k} \Delta t + \mathbf{J}_{r, j-1} \boldsymbol{\eta}_{gd, j-1} \Delta t \\ &= \Delta \tilde{\mathbf{R}}_{j-1 ,j}^\mathrm{T} \delta \boldsymbol{\phi}_{i, j-1} + \mathbf{J}_{r,j-1} \boldsymbol{\eta}_{gd, j-1} \Delta t \end{aligned}
\]
该式描述了如何从\(j-1\)时刻的噪声推断至\(j\)时刻。显然，这是一个线性系统。不妨设\(j-1\)时刻\(\delta \boldsymbol{\phi}_{i,j-1}\)的协方差为\(\boldsymbol{\Sigma}_{j-1}\)，\(\boldsymbol{\eta}_{gd}\)的协方差为\(\boldsymbol{\Sigma}_{\boldsymbol{\eta}_{gd}}\)，那么：
\[
\boldsymbol{\Sigma}_{j} = \Delta \tilde{\mathbf{R}}_{j-1 ,j}^\mathrm{T} \boldsymbol{\Sigma}_{j-1} \Delta \tilde{\mathbf{R}}_{j-1 ,j} + \mathbf{J}_{r,j-1} \boldsymbol{\Sigma}_{\boldsymbol{\eta}_{gd}} \mathbf{J}_{r,j-1}^\mathrm{T} \Delta t^2
\]
这表明预积分误差会随着累计变大，预积分观测量也会变得越来越不确定。
对于速度噪声量：
写成高斯噪声变量的线性组合形式：
\[
\delta \mathbf{v}_{ij} \approx \sum_{k=i}^{j-1} \left[ -\Delta \tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k -\mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t + \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad,k} \Delta t \right]
\]
\[
\begin{aligned} \delta \mathbf{v}_{ij} &= \sum_{k=i}^{j-1} \left[ -\Delta \tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k -\mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t + \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad,k} \Delta t \right] \\ &= \sum_{k=i}^{j-2} \left[ -\Delta \tilde{\mathbf{R}}_{ik}(\tilde{\mathbf{a}}_k -\mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t + \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad,k} \Delta t \right] \\ & \quad \quad \quad - \Delta \tilde{\mathbf{R}}_{i, j-1} (\tilde{\mathbf{a}}_{j-1} -\mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{i, j-1} \Delta t + \Delta \tilde{\mathbf{R}}_{i,j-1} \boldsymbol{\eta}_{ad, j-1} \Delta t \\ &= \delta \mathbf{v}_{i, j-1} - \Delta \tilde{\mathbf{R}}_{i,j-1} (\tilde{\mathbf{a}}_{j-1} -\mathbf{b}_{a, i})^\wedge \delta \boldsymbol{\phi}_{i, j-1} \Delta t + \Delta \tilde{\mathbf{R}}_{i, j-1} \boldsymbol{\eta}_{ad, j-1} \Delta t \end{aligned}
\]
于是\(\delta \mathbf{v}_{ij}\)的协方差也可以根据累加系数来确定。
对于位置噪声量：
\[
\begin{aligned} \delta \mathbf{p}_{ij} &= \sum_{k=i}^{j-1} \left[ \delta \mathbf{v}_{ik} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t^2 + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad,k} \Delta t^2 \right] \\ &= \sum_{k=i}^{j-2} \left[ \delta \mathbf{v}_{ik} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{ik} \Delta t^2 + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \boldsymbol{\eta}_{ad,k} \Delta t^2 \right] \\ & \quad \quad \quad + \delta \mathbf{v}_{i, j-1} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i, j-1} (\tilde{\mathbf{a}}_{j-1} - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{i,j-1} \Delta t^2 + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i,j-1} \boldsymbol{\eta}_{ad, j-1} \Delta t^2 \\ &= \delta \mathbf{p}_{i,j-1} + \delta \mathbf{v}_{i,j-1} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i, j-1} (\tilde{\mathbf{a}}_{j-1} - \mathbf{b}_{a,i})^\wedge \delta \boldsymbol{\phi}_{i,j-1} \Delta t^2 + \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i,j-1} \boldsymbol{\eta}_{ad, j-1} \Delta t^2 \end{aligned}
\]
于是，得到了如何从 j-1 时刻将噪声项递推至 j 时刻。整理成矩阵形式：
\[
\boldsymbol{\eta}_{ik} = \begin{bmatrix} \delta \boldsymbol{\phi}_{ik} \\ \delta \mathbf{v}_{ik} \\ \delta \mathbf{p}_{ik} \end{bmatrix}
\]
IMU的零偏噪声定义为：
\[
\boldsymbol{\eta}_{d,j} =\begin{bmatrix} \boldsymbol{\eta}_{gd, j} \\ \boldsymbol{\eta}_{ad, j} \end{bmatrix}
\]
那么从\(\boldsymbol{\eta}_{i,j-1}\)至\(\boldsymbol{\eta}_{i,j}\)的递推式可以写作：
\[
\boldsymbol{\eta}_{ij} = \mathbf{A}_j \boldsymbol{\eta}_{i,j-1} + \mathbf{B}_j \boldsymbol{\eta}_{d,j-1}
\]
\[
\mathbf{A}_j = \begin{bmatrix} \Delta \tilde{\mathbf{R}}_{j-1, j}^\mathrm{T} & \mathbf{0} & \mathbf{0} \\ -\Delta \tilde{\mathbf{R}}_{i, j-1} (\tilde{\mathbf{a}}_{j-1} - \mathbf{b}_{a,i})^\wedge \Delta t & \mathbf{I} & \mathbf{0} \\ - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i, j-1} (\tilde{\mathbf{a}}_{j-1} - \mathbf{b}_{a,i})^\wedge \Delta t^2 & \Delta t & \mathbf{I} \end{bmatrix}, \ \mathbf{B}_j = \begin{bmatrix} \mathbf{J}_{r,j-1} \Delta t & \mathbf{0} \\ \mathbf{0} & \Delta \tilde{\mathbf{R}}_{i, j-1} \Delta t \\ \mathbf{0} & \frac{1}{2} \Delta \tilde{\mathbf{R}}_{i,j-1} \Delta t^2 \end{bmatrix}
\]
零偏的更新
先前的讨论为了方便后续的计算都假设了在 i 时刻的IMU零偏恒定不变。在实际的图优化中，我们经常会对状态变量（优化变量）进行更新。那么，理论上来讲，如果IMU零偏发生了变化，预积分应该重新计算，因为预积分的每一步都用到了 i 时刻的IMU零偏。但是实际操作过程中，可以假定预积分观测是随零偏线性变化的，虽然实际上并不是线性变化的，但总可以对一个复杂函数做线性化并保留一阶项,然后在原先的观测量上进行修正。
把预积分观测量看成\(\mathbf{b}_{g,i}\),\(\mathbf{b}_{a,i}\)的函数，那么，当\(\mathbf{b}_{g,i}\),\(\mathbf{b}_{a,i}\)更新了\(\delta \mathbf{b}_{g,i}\),\(\delta \mathbf{b}_{a,i}\)之后，预积分观测应作如下的修正：
\[
\begin{aligned} \Delta \tilde{\mathbf{R}}_{ij}(\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i}) &= \Delta \tilde{\mathbf{R}}_{ij}(\mathbf{b}_{g,i}) \mathrm{Exp} \left(\frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g} \delta \mathbf{b}_{g,i}\right) \\ \Delta \tilde{\mathbf{v}}_{ij} (\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i}, \mathbf{b}_{a,i} + \delta \mathbf{b}_{a,i}) &= \Delta \tilde{\mathbf{v}}_{ij}(\mathbf{b}_{g,i}, \mathbf{b}_{a,i}) + \frac{\partial \Delta \tilde{\mathbf{v}}_{ij}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} + \frac{\partial \Delta \tilde{\mathbf{v}}_{ij}}{\partial \mathbf{b}_{a,i}} \delta \mathbf{b}_{a,i} \\ \Delta \tilde{\mathbf{p}}_{ij} (\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i}, \mathbf{b}_{a,i} + \delta \mathbf{b}_{a,i}) &= \Delta \tilde{\mathbf{p}}_{ij}(\mathbf{b}_{g,i}, \mathbf{b}_{a,i}) + \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} + \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{a,i}} \delta \mathbf{b}_{a,i} \end{aligned}
\]
旋转雅可比:
\[
\begin{aligned} \Delta \tilde{\mathbf{R}}_{ij} (\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i}) &= \prod_{k=i}^{j-1} \mathrm{Exp} \left((\tilde{\boldsymbol{\omega}}_k - (\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i})) \Delta t \right) \\ &= \prod_{k=i}^{j-1} \mathrm{Exp} \left((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_{g,i}) \Delta t \right) \mathrm{Exp}(-\mathbf{J}_{r,k} \delta \mathbf{b}_{g,i} \Delta t) \\ &= \underbrace{\mathrm{Exp} \left((\tilde{\boldsymbol{\omega}}_i - \mathbf{b}_{g,i}) \Delta t \right)}_{\Delta \tilde{\mathbf{R}}_{i,i+1}} \mathrm{Exp}(-\mathbf{J}_{r,i} \delta \mathbf{b}_{g,i} \Delta t) \underbrace{\mathrm{Exp} \left((\tilde{\boldsymbol{\omega}}_{i+1} - \mathbf{b}_{g,i}) \Delta t \right)}_{\Delta \tilde{\mathbf{R}}_{i+1, i+2}} \\ & \quad\quad\quad \mathrm{Exp}(-\mathbf{J}_{r,i+1} \delta \mathbf{b}_{g,i} \Delta t) \ldots \\ &= \Delta \tilde{\mathbf{R}}_{i, i+1} \Delta \tilde{\mathbf{R}}_{i+1, i+2} \mathrm{Exp}(-\Delta \tilde{\mathbf{R}}_{i+1, i+2}^\mathrm{T} \mathbf{J}_{r,i} \delta \mathbf{b}_{g,i} \Delta t) \ldots \\ &= \Delta \tilde{\mathbf{R}}_{ij} \prod_{k=i}^{j-1} \mathrm{Exp} \left( -\Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \delta \mathbf{b}_{g,i} \Delta t\right) \\ & \approx \Delta \tilde{\mathbf{R}}_{ij} \mathrm{Exp} \left( -\sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{r,k} \Delta t \delta \mathbf{b}_{g,i} \right) \end{aligned}
\]
最后一行用到了BCH在\(\delta \mathbf{b}_{g,i}\)为小量时雅可比接近单位阵的性质。通过这种方式我们可以算出\(\Delta \tilde{\mathbf{R}}_{ij}\)相对于\(\mathbf{b}_{g,i}\)的雅可比矩阵，记为\(\frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_g}\)。
速度雅克比：
\[
\begin{aligned} \Delta \tilde{\mathbf{v}}(\mathbf{b}_i + \delta \mathbf{b}_i) &= \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik}(\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i}) (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i} - \delta \mathbf{b}_{a,i}) \Delta t\\ &= \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \mathrm{Exp}\left( \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} \right) (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i} - \delta \mathbf{b}_{a,i}) \Delta t \\ &= \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \left(\mathbf{I} +\left( \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} \right)^\wedge \right)(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i} - \delta \mathbf{b}_{a,i}) \Delta t \\ & \approx \Delta \mathbf{v}_{ij} - \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \Delta t \delta \mathbf{b}_{a,i} - \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} (\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i})^\wedge \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \Delta t \delta \mathbf{b}_{g,i} \\ &= \Delta \mathbf{v}_{ij} + \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{a,i}} \delta \mathbf{b}_{a,i} + \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} \end{aligned}
\]
位置雅克比：
\[
\begin{aligned} \Delta \tilde{\mathbf{p}}_{ij}(\mathbf{b}_i + \delta \mathbf{b}_i) &= \sum_{k=i}^{j-1} \left[ \left(\Delta \tilde{\mathbf{v}}_{ik} + \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{a,i}} \delta \mathbf{b}_{a,i} + \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} \right) \Delta t + \right. \\ & \quad \quad \quad \left. \frac{1}{2}\Delta \tilde{\mathbf{R}}_{ik} \left(\mathbf{I} + \left( \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i} \right)^\wedge \right)(\tilde{\mathbf{a}}_k - \mathbf{b}_{a,i} -\delta \mathbf{b}_{a,i}) \Delta t^2 \right] \\ &= \delta \tilde{\mathbf{p}}_{ij} + \sum_{k=i}^{j-1}\left[\frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{a,i}} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \Delta t^2 \right] \delta \mathbf{b}_{a,i} + \\ & \quad \quad \quad \sum_{k=i}^{j-1} \left[\frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{g,i}} \Delta t -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{a,i}\right)^\wedge \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \Delta t^2 \right] \delta \mathbf{b}_{g,i} \\ &= \delta \tilde{\mathbf{p}}_{ij} + \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{a,i}} \delta \mathbf{b}_{a,i} + \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{g,i}} \mathbf{b}_{g,i} \end{aligned}
\]
整理得：
\[
\begin{align} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} &= -\sum_{k=i}^{j-1} \left[\Delta \tilde{\mathbf{R}}_{k+1, j}^\mathrm{T} \mathbf{J}_{k,r} \Delta t \right] \\ \frac{\partial \Delta \tilde{\mathbf{v}}_{ij}}{\partial \mathbf{b}_{a,i}} &= -\sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \Delta t \\ \frac{\partial \Delta \tilde{\mathbf{v}}_{ij}}{\partial \mathbf{b}_{g,i}} &= -\sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{ik} \left( \tilde{\mathbf{a}}_k - \mathbf{b}_{a,i} \right)^\wedge \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \Delta t \\ \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{a,i}} &= \sum_{k=i}^{j-1} \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{a,i}} \Delta t - \frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik} \Delta t^2 \\ \frac{\partial \Delta \tilde{\mathbf{p}}_{ij}}{\partial \mathbf{b}_{g,i}} &= \sum_{k=i}^{j-1} \frac{\partial \Delta \mathbf{v}_{ij}}{\partial \mathbf{b}_{g,i}} \Delta t -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{ik}\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{a,i}\right)^\wedge \frac{\partial \Delta \tilde{\mathbf{R}}_{ik}}{\partial \mathbf{b}_{g,i}} \Delta t^2 \end{align}
\]
预积分模型由于优化算法：
在IMU相关的应用中，通常把每个时刻的状态建模为包含旋转、平移、线速度、IMU零偏的变量，构成状态变量集合\(\mathcal{X}\)：
\[
\mathbf{x}_k = \left[ \mathbf{R}, \mathbf{p}, \mathbf{v}, \mathbf{b}_a, \mathbf{b}_g \right]_k \in \mathcal{X}
\]
而预积分模型构建了关键帧 i 与关键帧 k 之间的一种约束，它的残差可以写成：
\[
\begin{align} \mathbf{r}_{\Delta \mathbf{R}_{ij}} &= \mathrm{Log} \left(\Delta \tilde{\mathbf{R}}_{ij}^\mathrm{T} \left(\mathbf{R}_i^\mathrm{T} \mathbf{R}_j \right)\right) \\ \mathbf{r}_{\Delta \mathbf{v}_{ij}} &= \mathbf{R}_i^T \left(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij} \right) - \Delta \tilde{\mathbf{v}}_{ij} \\ \mathbf{r}_{\Delta \mathbf{p}_{ij}} &= \mathbf{R}_i^\mathrm{T} \left(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac{1}{2}\mathbf{g} \Delta t_{ij}^2 \right) - \Delta \tilde{\mathbf{p}}_{ij} \end{align}
\]
预积分相比于状态变量的雅可比矩阵:
旋转部分： 旋转与\(\mathbf{R}_{i}\),\(\mathbf{R}_j\)和\(\mathbf{b}_{g,i}\)有关。我们用\(\mathrm{SO}(3)\)的右扰动来推导它：
\[
\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{ij}}\left(\mathbf{R}_i \mathrm{Exp} (\boldsymbol{\phi}_i)\right) &= \mathrm{Log} \left( \Delta \tilde{\mathbf{R}}_{ij}^\mathrm{T} (\mathbf{R}_i \mathrm{Exp}(\boldsymbol{\phi_i})^\mathrm{T} \mathbf{R}_j) \right) \\ &= \mathrm{Log} \left( \Delta \tilde{\mathbf{R}}_{ij}^\mathrm{T} \mathrm{Exp} (-\boldsymbol{\phi}_i ) \mathbf{R}_i^\mathrm{T} \mathbf{R}_j \right) \\ &= \mathrm{Log} \left( \Delta \tilde{\mathbf{R}}_{ij}^\mathrm{T} \mathbf{R}_i^\mathrm{T} \mathbf{R}_j \mathrm{Exp} (-\mathbf{R}_j^\mathrm{T} \mathbf{R}_i \boldsymbol{\phi}_i) \right) \\ &= \mathbf{r}_{\Delta \mathbf{R}_{ij}} - \mathbf{J}_r^{-1} (\mathbf{r}_{\Delta \mathbf{R}_{ij}}) \mathbf{R}_j^\mathrm{T} \mathbf{R}_i \boldsymbol{\phi}_i \end{aligned}
\]
对\(\boldsymbol{\phi}_j\)的导数为：
\[
\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{ij}} (\mathbf{R}_j \mathrm{Exp}(\boldsymbol{\phi}_j)) &= \mathrm{Log}\left(\Delta \tilde{\mathbf{R}}_{ij}^\mathrm{T} \mathbf{R}^\mathrm{T}_j \mathbf{R}_j \mathrm{Exp} (\boldsymbol{\phi}_j ) \right) \\ &= \mathbf{r}_{\Delta \mathbf{R}_{ij}} + \mathbf{J}_r^{-1} (\mathbf{r}_{\Delta \mathbf{R}_{ij}}) \boldsymbol{\phi}_j \end{aligned}
\]
假设优化初始的零偏为\(\mathbf{b}_{g,i}\)，在某一步迭代时，我们当前估计出来的零偏修正为\(\delta \mathbf{b}_{g,i}\)，而当前修正得到的预积分旋转观测量为\(\Delta \tilde{\mathbf{R}}_{ij}^\prime = \Delta \tilde{\mathbf{R}}_{ij}(\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i})\)，残差为\(\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime\)。为了求导，我们又在上面两项基础上加上了\(\tilde{\delta} \mathbf{b}_{g,i}\)，那么：
\[
\begin{aligned} \mathbf{r}_{\Delta \mathbf{R}_{ij}} (\mathbf{b}_{g,i} + \delta \mathbf{b}_{g,i} + \tilde{\delta} \mathbf{b}_{g,i}) &= \mathrm{Log}\left( \left( \Delta \tilde{\mathbf{R}}_{ij} \mathrm{Exp} \left( \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} (\delta \mathbf{b}_{g,i} + \tilde{\delta} \mathbf{b}_{g,i}) \right) \right)^\mathrm{T} \mathbf{R}_i^\mathrm{T} \mathbf{R}_j \right) \\ &\text{BCH}\over= \mathrm{Log} \left( \left( \underbrace{\Delta \tilde{\mathbf{R}}_{ij} \mathrm{Exp} (\frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \delta \mathbf{b}_{g,i})}_{\Delta \tilde{\mathbf{R}}_{ij}^\prime} \mathrm{Exp} (\mathbf{J}_{r, b} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \tilde{\delta} \mathbf{b}_{g,i}) \right)^\mathrm{T} \mathbf{R}_i^\mathrm{T} \mathbf{R}_j \right) \\ &= \mathrm{Log} \left( \mathrm{Exp} \left(-\mathbf{J}_{b,r} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \tilde{\delta} \mathbf{b}_{g,i} \right) \underbrace{(\Delta \tilde{\mathbf{R}}_{ij}^\prime)^\mathrm{T} \mathbf{R}_i^\mathrm{T} \mathbf{R}_j}_{ \mathrm{Exp} \left( \mathbf{r}_{ \Delta \mathbf{R}_{ij}}^\prime \right)} \right) \\ &= \mathrm{Log} \left( \mathrm{Exp} \left(\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime \right) \mathrm{Exp} \left( - \mathrm{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime \right)^\mathrm{T} \mathbf{J}_{b,r} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \tilde{\delta} \mathbf{b}_{g,i} \right) \right) \\ &= \mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime - \mathbf{J}_r^{-1} (\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime ) \mathrm{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime \right)^\mathrm{T} \mathbf{J}_{b,r} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \tilde{\delta} \mathbf{b}_{g,i} \end{aligned}
\]
所以我们最后得到：
\[
\frac{\partial \mathbf{r}_{\Delta \mathbf{R}_{ij}}}{\partial \mathbf{b}_{g,i}} = - \mathbf{J}_r^{-1} (\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime ) \mathrm{Exp}\left(\mathbf{r}_{\Delta \mathbf{R}_{ij}}^\prime \right)^\mathrm{T} \mathbf{J}_{b,r} \frac{\partial \Delta \tilde{\mathbf{R}}_{ij}}{\partial \mathbf{b}_{g,i}} \tilde{\delta} \mathbf{b}_{g,i}
\]
速度部分： 速度项与\(\mathbf{v}_i\),\(\mathbf{v}_j\)呈线性关系，不难得到：
\[
\frac{ \partial \mathbf{r}_{\Delta \mathbf{v}_{ij}}}{\partial \mathbf{v}_i} = -\mathbf{R}_i^\mathrm{T}, \quad \frac{ \partial \mathbf{r}_{\Delta \mathbf{v}_{ij}}}{\partial \mathbf{v}_j} = \mathbf{R}_i^\mathrm{T}
\]
对旋转部分做一阶泰勒展开：
\[
\begin{aligned} \mathbf{r}_{\Delta \mathbf{v}_{ij}} \left( \mathbf{R}_i \mathrm{Exp} (\delta \boldsymbol{\phi}_i)\right) &= (\mathbf{R}_i \mathrm{Exp} (\Delta \boldsymbol{\phi}_i)) ^\mathrm{T} (\mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij}) - \Delta \tilde{\mathbf{v}}_{ij} \\ &= (\mathbf{I} - \Delta \boldsymbol{\phi}^\wedge_i) \mathbf{R}_i^\mathrm{T} (\mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij}) - \Delta \tilde{\mathbf{v}}_{ij} \\ &= \mathbf{r}_{\Delta \mathbf{v}_{ij}} (\mathbf{R}_i) + \left(\mathbf{R}_i^\mathrm{T} (\mathbf{v}_j - \mathbf{v}_i - \mathbf{g} \Delta t_{ij}) \right)^\mathrm{\wedge} \Delta \boldsymbol{\phi}_i \end{aligned}
\]
速度残差相对\(\mathbf{b}_{g,i}\),\(\mathbf{b}_{a,i}\)的雅可比只和\(\delta \tilde{\mathbf{v}}_{ij}\)相关。速度的残差项与它只相差一个负号。
位置部分： 位置部分和\(\mathbf{p}_i\),\(\mathbf{p}_j \mathbf{v}_i\),\(\mathbf{R}_i\)以及两个零偏有关。然而，它们的关系大多为线性关系，雅可比很容易推出。
\[
\begin{align} \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{ij}}}{\partial \mathbf{p}_i } &= -\mathbf{R}_i^\mathrm{T} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{ij}}}{\partial \mathbf{p}_j } &= \mathbf{R}_i^\mathrm{T} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{ij}}}{\partial \mathbf{v}_i } &= -\mathbf{R}_i^\mathrm{T} \Delta t_{ij} \\ \frac{\partial \mathbf{r}_{\Delta \mathbf{p}_{ij}}}{\partial \boldsymbol{\phi}_i } &= \left( \mathbf{R}_i^\mathrm{T} \left(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij} - \frac{1}{2}\mathbf{g} \Delta t_{ij}^2 \right) \right)^\wedge \end{align}
\]
零偏的残差也只需在零偏雅可比基础上添加负号即可。
Reference
[1] https://zhuanlan.zhihu.com/p/388859808
[2] https://github.com/PetWorm/IMU-Preintegration-Propogation-Doc