SLAM——IMU预积分

Posted on Edited on In

IMU预积分，耳熟能详，但是一直没有仔细了解。这里详细介绍一下。

运动模型

首先做一个简单规定，因为IMU预积分离不开IMU坐标系与世界坐标系的转换，因此我们将世界坐标下的值标记为$w$，将IMU坐标系下的值标记为$b$。而$\mathbf R^w_b$表示了从世界坐标到IMU坐标的转换。

一般来说，IMU包含了两部分——陀螺仪与加速计，因此IMU的raw data有两类：

$$
\tilde{\boldsymbol{\omega}}^{b}(t)=\boldsymbol{\omega}^{b}(t)+\mathbf{b}_ {g}(t)+\boldsymbol{\eta}_ {g}(t)\\tilde{\mathbf{a}}^{b}(t)=\mathbf{R}_ {b(t)}^{w T}\left(\mathbf{a}^{w}(t)-\mathbf{g}^{w}\right)+\mathbf{b}_ {a}(t)+\boldsymbol{\eta}_ {a}(t)
$$

其中$\tilde {\boldsymbol\omega}^b$与$\tilde{\mathbf{a}}^b$分别是读取到的角速度与加速度，也就是观察值，属于IMU坐标系下的读数，且包含噪声$\boldsymbol \eta$与偏差$\mathbf b$（分别是加速度accelerometer与陀螺仪gyro的噪声与偏差）。同时，IMU的加速度测量的是IMU坐标下的一个力，但是由于测量原理，它测出来的加速度去除了重力，例如一个三轴加速度计放在水平面上，那么它的输出将是铅垂线反向的$9.8m/s^2$；如果一个加速度计做自由落体运动，那么它的输出会是0。而我们关注的物体真正的加速度$\mathbf a ^w(t)$是世界坐标系$\mathbf W$下，当然也包含了重力。所以真实加速度需要去除重力加速度再转换到IMU坐标系下才是IMU的加速度读数。

同时，根据运动模型，我们可以得到旋转矩阵$\mathbf R$，速度$\mathbf v$以及位置$\mathbf p$的微分模型如下：

$$
\begin{array}{l}
\dot{\mathbf{R}}_ {b}^{w}=\mathbf{R}_ {b}^{w}\left(\boldsymbol{\omega}^{b}\right)^{\wedge} \
\dot{\mathbf{v}}^{w}=\mathbf{a}^{w} \
\dot{\mathbf{p}}^{w}=\mathbf{v}^{w}
\end{array}
$$

有意思的一件事是，虽然旋转矩阵是一个从世界坐标到IMU坐标的转换（也就世界坐标系下姿态的逆），但是微分里的角速度是IMU坐标系的，这意味着我们可以直接使用IMU去噪后的角速度读数。使用欧拉积分(？)，可以得到下一刻的状态（离散形式）：

$$
\begin{array}{l}\mathbf{R}_ {b(t+\Delta t)}^{w}=\mathbf{R}_ {b(t)}^{w} \operatorname{Exp}\left(\boldsymbol{\omega}^{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}
$$

上式中速度和位置都很容易理解，对于旋转矩阵，很显然上面给出的结果也是很符合直观的，就是将角速度与间隔时间相乘后得到一个旋转向量（角速度乘上时间也就得到了绕各个轴旋转的角度，可以直接得到旋转向量，所以一个旋转向量其实可以理解成绕坐标轴旋转的角度?），再转化到$\text{SO(3)}$上进行乘积。而一个严格的解释如下：

$$
\mathbf{R}^{w}_ {b(t)} \operatorname{Exp}\left(\boldsymbol{\omega}_ {w b}^{b}(t) \cdot \Delta t\right)=\mathbf{R}_ {b(t)}^{w} \mathbf{R}_ {b(t+\Delta t)}^{b(t)}=\mathbf{R}_ {b(t+\Delta t)}^{w}
$$

为了简化符号，规定：

$$
\mathbf{R}(t) \doteq \mathbf{R}_ {b(t)}^{w} ; \quad \boldsymbol{\omega}(t) \doteq \boldsymbol{\omega}^{b}(t) ; \quad \mathbf{a}(t)=\mathbf{a}^{b}(t) ; \\quad \mathbf{v}(t) \doteq \mathbf{v}^{w}(t) ; \quad \mathbf{p}(t) \doteq \mathbf{p}^{w}(t) ; \quad \mathbf{g} \doteq \mathbf{g}^{w}
$$

如果将测量模型带入到运动模型中，可以得到：

$$
\begin{aligned}\mathbf{R}(t+\Delta t) &=\mathbf{R}(t) \cdot \operatorname{Exp}(\boldsymbol{\omega}(t) \cdot \Delta t) \&=\mathbf{R}(t) \cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}(t)-\mathbf{b}_ {g}(t)-\boldsymbol{\eta}_ {g d}(t)\right) \cdot \Delta t\right) \\mathbf{v}(t+\Delta t) &=\mathbf{v}(t)+\mathbf{a}^{w}(t) \cdot \Delta t \&=\mathbf{v}(t)+\mathbf{R}(t) \cdot\left(\tilde{\mathbf{a}}(t)-\mathbf{b}_ {a}(t)-\boldsymbol{\eta}_ {a d}(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{a}^{w}(t) \cdot \Delta t^{2} \&=\mathbf{p}(t)+\mathbf{v}(t) \cdot \Delta t+\frac{1}{2}\left[\mathbf{R}(t) \cdot\left(\tilde{\mathbf{a}}(t)-\mathbf{b}_ {a}(t)-\boldsymbol{\eta}_ {a d}(t)\right)+\mathbf{g}\right] \cdot \Delta t^{2} \&=\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{a}}(t)-\mathbf{b}_ {a}(t)-\boldsymbol{\eta}_ {a d}(t)\right) \cdot \Delta t^{2}\end{aligned}
$$

首先要注意的是，上面的噪声也从连续形式变成了离散形式，而二者的区别就在于他们有不同的协方差：

$$
\begin{array}{l}\operatorname{Cov}\left(\boldsymbol{\eta}_ {g d}(t)\right)=\frac{1}{\Delta t} \operatorname{Cov}\left(\boldsymbol{\eta}_ {g}(t)\right) \\operatorname{Cov}\left(\boldsymbol{\eta}_ {a d}(t)\right)=\frac{1}{\Delta t} \operatorname{Cov}\left(\boldsymbol{\eta}_ {a}(t)\right)\end{array}
$$

如果假设采样频率一定，时间间隔为$\Delta t$，那么每个时刻可以表示为离散的$1, 2, …, k$，因此，进一步简化可以将下一个时刻的状态写为：

$$
\begin{array}{l}\mathbf{R}_ {k+1}=\mathbf{R}_ {k} \cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {k}^{g}-\boldsymbol{\eta}_ {k}^{g d}\right) \cdot \Delta t\right) \\mathbf{v}_ {k+1}=\mathbf{v}_ {k}+\mathbf{R}_ {k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\boldsymbol{\eta}_ {k}^{a d}\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{a}}_ {k}-\mathbf{b}_ {k}^{a}-\boldsymbol{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\end{array}
$$

预积分

根据之前的内容，如果有两个时刻$i,j$，且我们知道$i$时刻的机器人状态，以及$i,j$时刻之间的IMU状态，那么我们可以直接得到$j$时刻的机器人状态，通过对$i,j$时刻间的IMU状态进行上述的积分：

$$
\begin{aligned}\mathbf{R}_ {j} &=\mathbf{R}_ {i} \cdot \prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{g}-\mathbf{\eta}_ {k}^{g d}\right) \cdot \Delta t\right) \\mathbf{v}_ {j} &=\mathbf{v}_ {i}+\mathbf{g} \cdot \Delta t_ {i j}+\sum_ {k=i}^{j-1} \mathbf{R}_ {k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t \\mathbf{p}_ {j} &=\mathbf{p}_ {i}+\sum_ {k=i}^{j-1} \mathbf{v}_ {k} \cdot \Delta t+\frac{j-i}{2} \mathbf{g} \cdot \Delta t^{2}+\frac{1}{2} \sum_ {k=i}^{j-1} \mathbf{R}_ {k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2} \&=\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{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right]\end{aligned}
$$

其中$\Delta t_ {i j}=\sum_ {k=i}^{j-1} \Delta t=(j-i) \Delta t$，也就是时刻之间的时间间隔。可以看到，上述估计中相邻两个IMU数据之间的状态用先前的IMU状态来代替。后面的速度与位置的更新，都出现了新的变量$\mathbf R_k$，也就意味着当$\mathbf R_i$有变化，所有的$\mathbf R_k$都会跟着改变，所有的量都需重新计算。为了避免这样的情况，很直接的方法就是记录着$i,j$时刻之间的变化值，而这个变化值不应该随着$\mathbf R_i$
的改变而改变。因此，需要引入预积分。预积分的目的自然是为了消除IMU积分式中的$\mathbf R_ {i}$以及与其相关的$\mathbf R_k， \mathbf v_k$，这样之后$\mathbf R_i$改变后，可以直接利用先前计算的预积分得到更新的结果。因此我们定义下面为预积分的内容：

$$
\begin{aligned}\Delta \mathbf{R}_ {i j} & \triangleq \mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \&=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {k}^{g}-\boldsymbol{\eta}_ {k}^{g d}\right) \cdot \Delta t\right) \\Delta \mathbf{v}_ {i j} & \triangleq \mathbf{R}_ {i}^{T}\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right) \&=\sum_ {k=i}^{j-1} \Delta \mathbf{R}_ {i k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t \\Delta \mathbf{p}_ {i j} & \triangleq \mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right) \&=\sum_ {k=i}^{j-1}\left[\Delta \mathbf{v}_ {i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_ {i k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right]\end{aligned}
$$

预积分的这三个值，第一个容易理解以及第二个是容易理解的，就是在IMU积分项中把$\mathbf R_ {i}$，$\mathbf v_i$，$\mathbf p_i$提取出。而对于位置的预积分，更复杂一点，证明如下：

令$\xi_ {k}=\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}$，有

$$
\begin{aligned}& \mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2} \=& \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} \xi_ {k} \cdot \Delta t^{2}\right)-\sum_ {k=i}^{j-1} \mathbf{v}_ {i} \cdot \Delta t-\frac{(j-i)^{2}}{2} \mathbf{g} \cdot \Delta t^{2} \=& \sum_ {k=i}^{j-1}\left(\mathbf{v}_ {k}-\mathbf{v}_ {i}\right) \Delta t+\left[\frac{j-i}{2}-\frac{(j-i)^{2}}{2}\right] \mathbf{g} \cdot \Delta t^{2}+\frac{1}{2} \sum_ {k=i}^{j-1} \mathbf{R}_ {k} \xi_ {k} \cdot \Delta t^{2} \=& \sum_ {k=i}^{j-1}\left(\mathbf{v}_ {k}-\mathbf{v}_ {i}\right) \Delta t-\sum_ {k=i}^{j-1}(k-i) \mathbf{g} \cdot \Delta t^{2}+\frac{1}{2} \sum_ {k=i}^{j-1} \mathbf{R}_ {k} \xi_ {k} \cdot \Delta t^{2} \=& \sum_ {k=i}^{j-1}\left{\left[\mathbf{v}_ {k}-\mathbf{v}_ {i}-(k-i) \mathbf{g} \cdot \Delta t\right] \Delta t+\frac{1}{2} \mathbf{R}_ {k} \xi_ {k} \cdot \Delta t^{2}\right} \=& \sum_ {k=i}^{j-1}\left[\left(\mathbf{v}_ {k}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i k}\right) \Delta t+\frac{1}{2} \mathbf{R}_ {k} \xi_ {k} \cdot \Delta t^{2}\right] \=& \sum_ {k=i}^{j-1}\left[\mathbf{R}_ {i} \cdot \Delta \mathbf{v}_ {i k} \cdot \Delta t+\frac{1}{2} \mathbf{R}_ {k} \xi_ {k} \cdot \Delta t^{2}\right]\=&\mathbf R_i\sum_ {k=i}^{j-1}\left[\Delta \mathbf{v}_ {i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_ {i k} \cdot\left(\tilde{\mathbf{a}}_ {k}-\mathbf{b}_ {k}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right]\end{aligned}
$$

有了预积分项，想要得到积分后的结果就非常容易了。预积分的意义是找出$i,j$时刻之间IMU状态贡献的部分，即使机器人状态有变化，固定时刻之间IMU状态测量值不变，这个贡献也不应该改变，因此IMU预积分是可以当作两个机器人状态之间的一个约束。

预积分测量值以及测量噪声

这一部分是为了分离出噪声，直接使用测量值的预积分。首先假设$i, j$时刻之间的bias是一定的，那么

（1）对于$\Delta\mathbf R_ {ij}$有：

$$
\begin{aligned}\Delta \mathbf{R}_ {i j} &=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t-\boldsymbol{\eta}_ {k}^{g d} \Delta t\right) \& \approx \prod_ {k=i}^{j-1}\left{\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_ {r}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t\right) \cdot \mathbf{\eta}_ {k}^{g d} \Delta t\right)\right}\end{aligned}
$$

这一步是把$\boldsymbol \eta_ {k}^{gd}\Delta t$看作是一个微小量，则有$\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_ {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right)$。

从这一步继续往下推导会稍微复杂一点，先给出结果：

为了符号简便，令：

$$
\mathbf{J}_ {r}^{k}=\mathbf{J}_ {r}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t\right)\\Delta \tilde{\mathbf{R}}_ {i j}=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t\right)\\operatorname{Exp}\left(-\delta \vec{\phi}_ {i j}\right)=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {k+1,j}^{T} \cdot \mathbf{J}_ {r}^{k} \cdot \boldsymbol{\eta}_ {k}^{g d} \Delta t\right)
$$

则有：

$$
\Delta \mathbf{R}_ {i j} = \Delta \tilde{\mathbf{R}}_ {i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_ {i j}\right)
$$

其中$\Delta\tilde{\mathbf R}_ {ij}$就是旋转量的预积分测量值，后者为噪声。

下面我们证明。首先，我们利用$\text{SO(3)}$的伴随性质可以得到：

$$
\operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}=\mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right)
$$

这里我们对上式进行一个推广：

$$
\begin{aligned}\operatorname{Exp}(\vec{\phi}_1)\operatorname{Exp}(\vec{\phi}_2) \cdot \mathbf{R}&=\operatorname{Exp}(\vec{\phi}_1)\mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}_2\right)\&=\mathbf R \operatorname{Exp}(\mathbf R^T\vec{\phi}_1)\operatorname{Exp}(\mathbf R^T\vec{\phi}_2)\end{aligned}
$$

也就是，伴随性质是链式传播的，从最右侧传到最左侧，中间每一个量都会受到影响。为了符号简便，我们可以令$\mathbf R_ {k,k+1}=\operatorname{Exp}\left(\tilde{\boldsymbol{\omega}}_ {k}-\mathbf{b}_ {i}^{g}\right) \Delta t$，也就是：

$$
\Delta \tilde{\mathbf{R}}_ {i j}=\prod_ {k=i}^{j-1} \mathbf R_ {k,k+1}
$$

则有：

$$
\begin{aligned}\Delta \mathbf{R}_ {i j}&=\prod_ {k=i}^{j-1} \left{\mathbf R_ {k,k+1}\cdot\text{Exp}(-\mathbf{J}_ {r}^{k} \cdot \boldsymbol{\eta}_ {k}^{g d} \Delta t) \right}\&=\mathbf R_ {i, i+1}\cdot\text{Exp}(-\mathbf{J}_ {r}^{i} \cdot \boldsymbol{\eta}_ {i}^{g d} \Delta t)\cdot\mathbf R_ {i+1, i+2}\cdot\text{Exp}(-\mathbf{J}_ {r}^{i+1} \cdot \boldsymbol{\eta}_ {i+1}^{g d} \Delta t)\cdots\mathbf R_ {j-1,j}\cdot \text{Exp}(-\mathbf{J}_ {r}^{j-1} \cdot \boldsymbol{\eta}_ {j-1}^{g d} \Delta t) \ &= \mathbf R_ {i,i+1}\cdot \mathbf R_ {i+1, i+2} \cdot\text{Exp}(-\mathbf R_ {i+1,i+2}^T\cdot\mathbf{J}_ {r}^{i} \cdot \boldsymbol{\eta}_ {i}^{g d} \Delta t)\cdot\text{Exp}(-\mathbf{J}_ {r}^{i+1} \cdot \boldsymbol{\eta}_ {i+1}^{g d} \Delta t)\mathbf R_ {i+2, i+3}\cdots\mathbf R_ {j-1,j}\cdot \text{Exp}(-\mathbf{J}_ {r}^{j-1} \cdot \boldsymbol{\eta}_ {j-1}^{g d} \Delta t)\&= \mathbf R_ {i,i+1}\cdot \mathbf R_ {i+1, i+2} \cdot \mathbf R_ {i+2, i+3}\cdot\text{Exp}(-\mathbf R_ {i+2,i+3}^T\mathbf R_ {i+1,i+2}^T\cdot\mathbf{J}_ {r}^{i} \cdot \boldsymbol{\eta}_ {i}^{g d} \Delta t)\cdot\text{Exp}(-\mathbf R_ {i+2,i+3}^T\mathbf{J}_ {r}^{i+1} \cdot \boldsymbol{\eta}_ {i+1}^{g d} \Delta t)\cdots\mathbf R_ {j-1,j}\cdot \text{Exp}(-\mathbf{J}_ {r}^{j-1} \cdot \boldsymbol{\eta}_ {j-1}^{g d} \Delta t)\&\cdots\&=\prod_ {k=i}^{j-1} \mathbf R_ {k,k+1} \cdot \text{Exp}\left(-\left(\prod_ {k=i+1}^{j-1}\mathbf{R}_ {k,k+1}\right)^T\mathbf{J}_ {r}^{i} \cdot \boldsymbol{\eta}_ {i}^{g d} \Delta t \right) \cdots \text{Exp}(-\mathbf{J}_ {r}^{j-1} \cdot \boldsymbol{\eta}_ {j-1}^{g d} \Delta t)\&=\Delta\tilde{\mathbf R}_ {ij}\cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {i+1,j}^{T} \cdot \mathbf{J}_ {r}^{i} \cdot \boldsymbol{\eta}_ {i}^{g d} \Delta t\right)\cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {i+2,j}^{T} \cdot \mathbf{J}_ {r}^{i+1} \cdot \boldsymbol{\eta}_ {i+1}^{g d} \Delta t\right)\cdots \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {j,j}^{T} \cdot \mathbf{J}_ {r}^{j-1} \cdot \boldsymbol{\eta}_ {j-1}^{g d} \Delta t\right)\&=\Delta \tilde{ \mathbf R}_ {ij}\cdot\prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {k+1,j}^{T} \cdot \mathbf{J}_ {r}^{k} \cdot \boldsymbol{\eta}_ {k}^{g d} \Delta t\right)\& = \Delta \tilde{\mathbf{R}}_ {i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_ {i j}\right)\end{aligned}
$$

证毕，需要注意的是$\Delta\tilde{\mathbf R}_ {jj} = \mathbf I$。

（2）对于$\Delta \mathbf v_ {ij}$，将（1）中的结论带入有：

$$
\begin{aligned}\Delta \mathbf{v}_ {i j} &=\sum_ {k=i}^{j-1} \Delta \mathbf{R}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t \& \approx \sum_ {k=i}^{j-1} \Delta \tilde{\mathbf{R}}_ {i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_ {i k}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t \& \approx \sum_ {k=i}^{j-1} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_ {i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\boldsymbol{\eta}_ {k}^{a d}\right) \cdot \Delta t \& \approx \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_ {i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \cdot \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \boldsymbol{\eta}_ {k}^{a d} \Delta t\right] \&=\sum_ {k=i}^{a-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \cdot \Delta t+\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t\right] \&=\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \cdot \Delta t\right]+\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t\right]\end{aligned}
$$

令：

$$
\begin{aligned}\Delta \tilde{\mathbf{v}}_ {i j} & \triangleq \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \cdot \Delta t\right] \\delta \mathbf{v}_ {i j} & \triangleq \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t\right]\end{aligned}
$$

则有：

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

其中前一项为测量项，而后一项为噪声值。

（3）对于$\Delta\mathbf {p}_ {ij}$，带入前面的结果，可得：

$$
\begin{aligned}\Delta \mathbf{p}_ {i j} &=\sum_ {k=i}^{j-1}\left[\Delta \mathbf{v}_ {i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right] \& \approx \sum_ {k=i}^{j-1}\left[\left(\Delta \tilde{\mathbf{v}}_ {i k}-\delta \mathbf{v}_ {i k}\right) \cdot \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_ {i k}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right] \& \approx \sum_ {k=i}^{(1)}\left[\left(\Delta \tilde{\mathbf{v}}_ {i k}-\delta \mathbf{v}_ {i k}\right) \cdot \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_ {i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}-\mathbf{\eta}_ {k}^{a d}\right) \cdot \Delta t^{2}\right] \& \approx \sum_ {k=i}^{-1}\left[\left(\Delta \tilde{\mathbf{v}}_ {i k}-\delta \mathbf{v}_ {i k}\right) \cdot \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_ {i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \cdot \Delta t^{2}-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t^{2}\right] \& = \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_ {i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \delta \vec{\phi}_ {i k} \Delta t^{2}-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t^{2}-\delta \mathbf{v}_ {i k} \Delta t\right]\end{aligned}
$$

令：

$$
\begin{array}{l}\Delta \tilde{\mathbf{p}}_ {i j} \triangleq \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_ {i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right) \Delta t^{2}\right] \\delta \mathbf{p}_ {i j} \triangleq \sum_ {k=i}^{j-1}\left[\delta \mathbf{v}_ {i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \delta \vec{\phi}_ {i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \mathbf{n}_ {k}^{a d} \Delta t^{2}\right]\end{array}
$$

则

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

前者为预积分测量值，后者为噪声值。

最终，将分离后的噪声与测量值带入理想状态的预积分形式下，可以得到：

$$
\begin{array}{l}\Delta \tilde{\mathbf{R}}_ {i j} \approx \Delta \mathbf{R}_ {i j} \operatorname{Exp}\left(\delta \vec{\phi}_ {i j}\right)=\mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \operatorname{Exp}\left(\delta \vec{\phi}_ {i j}\right) \\Delta \tilde{\mathbf{v}}_ {i j} \approx \Delta \mathbf{v}_ {i j}+\delta \mathbf{v}_ {i j}=\mathbf{R}_ {i}^{T}\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)+\delta \mathbf{v}_ {i j} \\Delta \tilde{\mathbf{p}}_ {i j} \approx \Delta \mathbf{p}_ {i j}+\delta \mathbf{p}_ {i j}=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)+\delta \mathbf{p}_ {i j}\end{array}
$$

类似于测量值等于理想值+噪声的形式。

在实现的时候需要注意，$\Delta \tilde{\mathbf v}_ {ij}$会用到的是$\sum_ {k=i}^{j-1}\Delta \tilde{\mathbf{R}}_ {i k} = \Delta \tilde{\mathbf{R}}_ {i i}+\cdots+\Delta \tilde{\mathbf{R}}_ {i,j-1}$，也就是不会用到$\Delta \tilde{\mathbf{R}}_ {i j}$，因此实现的时候需要注意计算顺序：$\Delta \tilde{\mathbf{p}}_ {i j}\rightarrow\Delta \tilde{\mathbf{v}}_ {i j}\rightarrow \Delta \tilde{\mathbf{R}}_ {i j}$。

噪声分布

令预积分的噪声为：

$$
\boldsymbol{\eta}_ {i j}^{\Delta} \triangleq\left[\begin{array}{lll}\delta \vec{\boldsymbol\phi}_ {i j}^{T} & \delta \mathbf{v}_ {i j}^{T} & \delta \mathbf{p}_ {i j}^{T}\end{array}\right]^{T}
$$

我们希望它满足高斯分布，也就是$\boldsymbol{\eta}_ {i j}^{\Delta} \sim N\left(\mathbf{0}_ {9 \times 1}, \mathbf{\Sigma}_ {i j}\right)$。这里$\boldsymbol \eta_ {ij}^\Delta$是三种噪声的线性组合，因此我们来分别分析。

（1）首先是旋转的噪声分析$\delta\boldsymbol{\vec\phi}$。由之前的推导可以知道：

$$
\operatorname{Exp}\left(-\delta \vec{\phi}_ {i j}\right)=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {k+1 j}^{T} \cdot \mathbf{J}_ {r}^{k} \cdot \boldsymbol{\eta}_ {k}^{g d} \Delta t\right)
$$

对等式两边同时取对数，可以得到：

$$
\delta \vec{\phi}_ {i j}=-\text{Log}\left(\prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_ {k+1 j}^{T} \cdot \mathbf{J}_ {r}^{k} \cdot \boldsymbol{\eta}_ {k}^{g d} \Delta t\right)\right)
$$

令$\xi_ {k}=\Delta \tilde{\mathbf{R}}_ {k+1 j}^{T} \cdot \mathbf{J}_ {r}^{k} \cdot \mathbf{\eta}_ {k}^{g d} \Delta t$，由于$\boldsymbol{\eta}_ {k}^{g d}$是小量，因此$\xi_k$也是小量，因此有$\mathbf{J}_ {r}\left(\xi_ {k}\right) \approx \mathbf{I}$(注意这里是雅可比矩阵，而不是之前的缩写)。由于本身$\delta \vec{\phi}_ {i j}$是小量，因此任意$\text{Log} \left(\prod_ {k =p}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right), p \in {i, i+1,…,j-1}$是小量，因为有$\text{Log} (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))=\vec{\phi}+\mathbf{J}_ {r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}$，我们可以得到

$$
\begin{aligned}\delta \vec{\phi}_ {i j} &=-\text{Log}\left(\prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right) \&=-\text{Log}\left(\operatorname{Exp}\left(-\xi_ {i}\right) \prod_ {k=i+1}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right) \& \approx-\left(-\xi_ {i}+\mathbf{I} \cdot \text{Log}\left(\prod_ {k=1+1}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right)\right)=\xi_ {i}-\text{Log} \left(\prod_ {k=i+1}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right) \&=\xi_ {i}-\text{Log}\left(\operatorname{Exp}\left(-\xi_ {i+1}\right) \prod_ {k=i+2}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right) \& \approx \xi_ {i}+\xi_ {i+1}-\text{Log} \left(\prod_ {k=1+2}^{j-1} \operatorname{Exp}\left(-\xi_ {k}\right)\right) \& \approx \cdots \& \approx \sum_ {k=i}^{j-1} \xi_ {k}\end{aligned}
$$

也就是

$$
\delta \vec{\phi}_ {i j} \approx \sum_ {k=i}^{j-1} \Delta \tilde{\mathbf{R}}_ {k+1 j}^{T} \mathbf{J}_ {r}^{k} \mathbf{\eta}_ {k}^{g d} \Delta t
$$

由于上式中$\Delta \tilde{\mathbf{R}}_ {k+1 j}^{T}, \mathbf{J}_ {r}^{k},\Delta t$都是已知量，而假设$\boldsymbol \eta_k^{gd}$是零均值高斯噪声，因此$\delta\vec{\phi}_ {ij}$也是零均值高斯噪声。

（2）由于我们分析得到了$\delta\vec{\phi}_ {ij}$是零均值高斯噪声，根据$\delta\mathbf v_ {ij}$表达式：

$$
\delta \mathbf{v}_ {i j}=\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t\right]
$$

我们可以知道$\delta\mathbf v_ {ij}$也是高斯分布的。

（3）类似的，根据$\delta \mathbf{p}_ {i j}$的表达式：

$$
\delta \mathbf{p}_ {i j}=\sum_ {k=i}^{j-1}\left[\delta \mathbf{v}_ {i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \delta \vec{\phi}_ {i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t^{2}\right]
$$

可以知道$\delta \mathbf{p}_ {i j}$也是高斯分布。但是，$\delta \mathbf{v}_ {i j},\delta \mathbf{p}_ {i j}$不是零均值。

噪声的递推形式

下面分析一下噪声的递推形式，$\boldsymbol\eta_ {i,j-1}^\Delta\rightarrow\boldsymbol \eta_ {ij}^{\Delta}$，以及其协方差的递推形式，$\boldsymbol{\Sigma}_ {i j-1} \rightarrow \boldsymbol{\Sigma}_ {i j}$。

（1）旋转项$\delta \vec{\phi}_ {i,j-1}\rightarrow \delta \vec{\phi}_ {ij}$：

$$
\begin{aligned}\delta \vec{\phi}_ {i j} &=\sum_ {k=1}^{j-1} \Delta \tilde{\mathbf{R}}_ {k+1 j}^{T} \mathbf{J}_ {r}^{k} \mathbf{\eta}_ {k}^{g d} \Delta t \&=\sum_ {k=i}^{j-2} \Delta \tilde{\mathbf{R}}_ {k+1,j}^{T} \mathbf{J}_ {r}^{k} \mathbf{\eta}_ {k}^{g d} \Delta t+\Delta \tilde{\mathbf{R}}_ {j j}^{T} \mathbf{J}_ {r}^{j-1} \mathbf{\eta}_ {j-1}^{g d} \Delta t \&=\sum_ {k=1}^{j-2}\left(\Delta \tilde{\mathbf{R}}_ {k+1,j-1} \Delta \tilde{\mathbf{R}}_ {j-1,j}\right)^{T} \mathbf{J}_ {r}^{k} \mathbf{\eta}_ {k}^{g d} \Delta t+\mathbf{J}_ {r}^{j-1} \mathbf{\eta}_ {j-1}^{g d} \Delta t \&=\Delta \tilde{\mathbf{R}}_ {j,j-1} \sum_ {k=1}^{j-2} \Delta \tilde{\mathbf{R}}_ {k+1,j-1}^{T} \mathbf{J}_ {r}^{k} \mathbf{\eta}_ {k}^{g d} \Delta t+\mathbf{J}_ {r}^{j-1} \mathbf{\eta}_ {j-1}^{g d} \Delta t \&=\Delta \tilde{\mathbf{R}}_ {j,j-1} \delta \vec{\phi}_ {i j-1}+\mathbf{J}_ {r}^{j-1} \mathbf{\eta}_ {j-1}^{g d} \Delta t\end{aligned}
$$

（2）速度项$\delta \mathbf{v}_ {i,j-1} \rightarrow \delta \mathbf{v}_ {i j}$:

$$
\begin{aligned}\delta \mathbf{v}_ {i j}=& \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k} \boldsymbol{\eta}_ {k}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t\right] \=& \sum_ {k=i}^{j-2}\left[\Delta \tilde{\mathbf{R}}_ {i k} \mathbf{p}_ {k}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i k} \cdot \Delta t\right]\&+\Delta \tilde{\mathbf{R}}_ {i,j-1} \mathbf{\eta}_ {j-1}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i,j-1} \cdot \Delta t \=& \delta \mathbf{v}_ {i,j-1}+\Delta \tilde{\mathbf{R}}_ {i,j-1} \mathbf{\eta}_ {j-1}^{a d} \Delta t-\Delta \tilde{\mathbf{R}}_ {i,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_ {i,j-1} \cdot \Delta t\end{aligned}
$$

（3）位置项$\delta \mathbf{p}_ {i,j-1} \rightarrow \delta \mathbf{p}_ {i j}$：

$$
\begin{aligned}\delta \mathbf{p}_ {i j} &=\sum_ {k=i}^{j-1}\left[\delta \mathbf{v}_ {i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \delta \vec{\phi}_ {i k} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k} \mathbf{\eta}_ {k}^{a d} \Delta t^{2}\right] \&=\delta \mathbf{p}_ {i,j-1}+\delta \mathbf{v}_ {i,j-1} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \delta \vec{\phi}_ {i,j-1} \Delta t^{2}+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i,j-1} \mathbf{\eta}_ {j-1}^{a d} \Delta t\end{aligned}
$$

综上所述，可以得到噪声预积分的递推形象如下（令$\mathbf{\eta}_ {k}^{d}=\left[\left(\boldsymbol{\eta}_ {k}^{g d}\right)^{T}\left(\boldsymbol{\eta}_ {k}^{\mathrm{ad}}\right)^{T}\right]^{T}$）：

$$
\begin{aligned}\boldsymbol{\eta}_ {i j}^{\Delta}&=\left[\begin{array}{ccc}\Delta \tilde{\mathbf{R}}_ {j, j-1} & \mathbf{0} & \mathbf{0} \-\Delta \tilde{\mathbf{R}}_ {i ,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \Delta t & \mathbf{I} & \mathbf{0} \-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i, j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \Delta t^{2} & \Delta t \mathbf I & \mathbf{I}\end{array}\right] \boldsymbol{\eta}_ {i ,j-1}^{\Delta} \&~+\left[\begin{array}{cc}
\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{array}\right] \boldsymbol{\eta}_ {j-1}^{d}\&=\mathbf{A}_ {j-1} \boldsymbol{\eta}_ {i ,j-1}^{\Delta}+\mathbf{B}_ {j-1} \boldsymbol{\eta}_ {j-1}^{d}\end{aligned}
$$

其中：

$$
\begin{aligned}&\mathbf{A}_ {j-1}=\left[\begin{array}{ccc}\Delta \tilde{\mathbf{R}}_ {j,j-1} & \mathbf{0} & \mathbf{0} \-\Delta \tilde{\mathbf{R}}_ {i,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \Delta t & \mathbf{I} & \mathbf{0} \-\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i,j-1} \cdot\left(\tilde{\mathbf{f}}_ {j-1}-\mathbf{b}_ {i}^{a}\right)^{\wedge} \Delta t^{2} & \Delta t \mathbf{I} & \mathbf{I}\end{array}\right]\&\mathbf{B}_ {j-1}=\left[\begin{array}{cc}\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{array}\right]\end{aligned}
$$

而噪声的协方差也就有了下面的递推形式：

$$
\boldsymbol{\Sigma}_ {i j}=\mathbf{A}_ {j-1} \boldsymbol{\Sigma}_ {i j-1} \mathbf{A}_ {j-1}^{T}+\mathbf{B}_ {j-1} \boldsymbol{\Sigma}_ {\mathbf{\eta}} \mathbf{B}_ {j-1}^{T}
$$

bias更新

目前为止做的计算都是假设了$i,j$时刻之间的bias是不变化的。如果bias发生了更新，需要将预计分测量值整个重新计算（bias在这段时刻里依然是恒定的，但是bias的值可能会被优化等方式更新）。目前的做法是将这个过程线性化，来得到一阶近似结果。现在先规定一些符号，首先我们将旧的bias标记为$\overline{\mathbf{b}}_ {i}^{g} ,\overline{\mathbf{b}}_ {i}^{a}$，而新的bias$\hat{\mathbf{b}}_ {i}^{g} , \hat{\mathbf{b}}_ {i}^{a}$是由旧的bias加上更新量$\delta\mathbf{b}_ {i}^{g}, \delta\mathbf{b}_ {i}^{a}$得到的，即：

$$
\hat{\mathbf{b}}_ {i}^{g} \leftarrow \overline{\mathbf{b}}_ {i}^{g}+\delta \mathbf{b}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a} \leftarrow \overline{\mathbf{b}}_ {i}^{a}+\delta \mathbf{b}_ {i}^{a}
$$

则有一阶近似如下（类似于性质$\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_ {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right)$）：

$$
\begin{array}{l}\Delta \tilde{\mathbf{R}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \approx \Delta \tilde{\mathbf{R}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \\Delta \tilde{\mathbf{v}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g} \hat{\mathbf{b}}_ {i}^{a}\right) \approx \Delta \tilde{\mathbf{v}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a} \\Delta \tilde{\mathbf{p}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right) \approx \Delta \tilde{\mathbf{p}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}\end{array}
$$

如果做符号简化如下：

$$
\begin{array}{l}\Delta \hat{\mathbf{R}}_ {i j} \doteq \Delta \tilde{\mathbf{R}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}\right), \Delta \overline{\mathbf{R}}_ {i j} \doteq \Delta \tilde{\mathbf{R}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}\right) \\Delta \hat{\mathbf{v}}_ {i j} \doteq \Delta \tilde{\mathbf{v}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right), \Delta \overline{\mathbf{v}}_ {i j} \doteq \Delta \tilde{\mathbf{v}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right) \\Delta \hat{\mathbf{p}}_ {i j} \doteq \Delta \tilde{\mathbf{p}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right), \Delta \overline{\mathbf{p}}_ {i j} \doteq \Delta \tilde{\mathbf{p}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right)\end{array}
$$

就可以写成：

$$
\begin{array}{l}\Delta \hat{\mathbf{R}}_ {i j} \approx \Delta \overline{\mathbf{R}}_ {i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \\Delta \hat{\mathbf{v}}_ {i j} \approx \Delta \overline{\mathbf{v}}_ {i j}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a} \\Delta \hat{\mathbf{p}}_ {i j} \approx \Delta \overline{\mathbf{p}}_ {i j}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}\end{array}
$$

下面我们需要推导的是式子中的各个偏导数。

（1）旋转项$\Delta \hat{\mathbf{R}}_ {i j} \approx \Delta \overline{\mathbf{R}}_ {i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathrm{b}}^{y}} \delta \mathbf{b}_ {i}^{g}\right)$：

$$
\begin{aligned}\Delta \hat{\mathbf{R}}_ {i j} &=\Delta \tilde{\mathbf{R}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \&=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\hat{\mathbf{b}}_ {i}^{g}\right) \Delta t\right) \&=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\left(\overline{\mathbf{b}}_ {i}^{g}+\delta \mathbf{b}_ {i}^{g}\right)\right) \Delta t\right) \&=\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\overline{\mathbf{b}}_ {i}^{g}\right) \Delta t-\delta \mathbf{b}_ {i}^{g} \Delta t\right) \& \approx \prod_ {k=i}^{j-1}\left(\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\overline{\mathbf{b}}_ {i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_ {r}^{k} \delta \mathbf{b}_ {i}^{g} \Delta t\right)\right)\end{aligned}
$$

到了这一步后，使用的变形手法与之前分离测量值与噪声的时候类似，我们依然利用伴随性质来处理。先做一些符号简化，令：

$$
\mathbf{M}_ {k}=\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\overline{\mathbf{b}}_ {i}^{g}\right) \Delta t\right)\\operatorname{Exp}\left(\mathbf{d}_ {k}\right)=\operatorname{Exp}\left(-\mathbf{J}_ {r}^{k} \delta \mathbf{b}_ {i}^{g} \Delta t\right)
$$

则有：

$$
\Delta\hat{\mathbf R}_ {ij} = \mathbf{M}_ {i} \cdot \operatorname{Exp}\left(\mathbf{d}_ {i}\right) \cdot \mathbf{M}_ {i+1} \cdot \operatorname{Exp}\left(\mathbf{d}_ {i+1}\right) \cdots \mathbf{M}_ {j-2} \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-2}\right) \cdot \mathbf{M}_ {j-1} \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-1}\right)
$$

根据伴随性质，我们可以得到：

$$
\begin{array}{l}\mathbf{M}_ {i} \cdot \operatorname{Exp}\left(\mathbf{d}_ {i}\right) \cdot \mathbf{M}_ {i+1} \cdot \operatorname{Exp}\left(\mathbf{d}_ {i+1}\right) \cdots \mathbf{M}_ {j-2} \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-2}\right) \cdot \mathbf{M}_ {j-1} \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-1}\right) \=\mathbf{M}_ {i} \cdot\left(\operatorname{Exp}\left(\mathbf{d}_ {i}\right) \cdot \mathbf{M}_ {i+1}\right) \cdot \operatorname{Exp}\left(\mathbf{d}_ {i+1}\right) \cdots \mathbf{M}_ {j-2} \cdot\left(\operatorname{Exp}\left(\mathbf{d}_ {j-2}\right) \cdot \mathbf{M}_ {j-1}\right) \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-1}\right) \=\mathbf{M}_ {i} \cdot\left(\mathbf{M}_ {i+1} \operatorname{Exp}\left(\mathbf{M}_ {i+1}^{T} \mathbf{d}_ {i}\right)\right) \cdot \operatorname{Exp}\left(\mathbf{d}_ {i+1}\right) \cdots \mathbf{M}_ {j-2} \cdot\left(\operatorname{Exp}\left(\mathbf{d}_ {j-2}\right) \cdot \mathbf{M}_ {j-1}\right) \cdot \operatorname{Exp}\left(\mathbf{d}_ {j-1}\right) \=\mathbf{M}_ {i} \mathbf{M}_ {i+1} \mathbf{M}_ {i+2} \operatorname{Exp}\left(\mathbf{M}_ {i+2}^{T} \mathbf{M}_ {i+1}^{T} \mathbf{d}_ {i}\right) \operatorname{Exp}\left(\mathbf{M}_ {i+2}^{T} \mathbf{d}_ {i+1}\right) \cdots \=\cdots \=\left(\prod_ {k=i}^{j-1} \mathbf{M}_ {k}\right) \cdot\left(\operatorname{Exp}\left(\left(\prod_ {k=i+1}^{j-1} \mathbf{M}_ {k}\right)^{T} \mathbf{d}_ {i}\right)\right) \cdot\left(\operatorname{Exp}\left(\left(\prod_ {k=i+2}^{j-1} \mathbf{M}_ {k}\right)^{T} \mathbf{d}_ {i+1}\right)\right) \cdots\left(\operatorname{Exp}\left(\left(\prod_ {k=j-1}^{j-1} \mathbf{M}_ {k}\right)^{T} \mathbf{d}_ {j-2}\right)\right) \cdot\left(\operatorname{Exp}\left(\mathbf{d}_ {j-1}\right)\right)\end{array}
$$

由于$\prod_ {k=m}^{j-1} \mathbf{M}_ {k}=\prod_ {k=m}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_ {k}-\overline{\mathbf{b}}_ {i}^{g}\right) \Delta t\right)=\Delta \overline{\mathbf{R}}_ {m j}$，则上式可以写为：

$$
\begin{array}{l}\Delta \overline{\mathbf{R}}_ {i j} \operatorname{Exp}\left(\Delta \overline{\mathbf{R}}_ {i+1 ,}^{T} \mathbf{d}_ {i}\right) \operatorname{Exp}\left(\Delta \overline{\mathbf{R}}_ {i+2,j}^{T} \mathbf{d}_ {i+1}\right) \cdots \operatorname{Exp}\left(\Delta \overline{\mathbf{R}}_ {j-1,j}^{T} \mathbf{d}_ {j-2}\right) \operatorname{Exp}\left(\Delta \overline{\mathbf{R}}_ {j j}^{T} \mathbf{d}_ {j-1}\right) \=\Delta \overline{\mathbf{R}}_ {i j} \prod_ {k=i}^{j-1} \operatorname{Exp}\left(\Delta \overline{\mathbf{R}}_ {k+1,j}^{T} \mathbf{d}_ {k}\right)\end{array}
$$

也就是：

$$
\Delta \hat{\mathbf{R}}_ {i j}=\Delta \overline{\mathbf{R}}_ {i j} \prod_ {k=i}^{j-1} \operatorname{Exp}\left(-\Delta \overline{\mathbf{R}}_ {k+1 j}^{T} \mathbf{J}_ {r}^{k} \delta \mathbf{b}_ {i}^{g} \Delta t\right)
$$

令$\mathbf{c}_ {k}=-\Delta \overline{\mathbf{R}}_ {k+1 j}^{T} \mathbf{J}_ {r}^{k} \delta \mathbf{b}_ {i}^{g} \Delta t$，由于$\delta \mathbf b_ {i}^g$很小，所以$\mathbf {c}k$很小，可以利用性质$\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J} {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right)$以及$\mathbf J_r(\delta\vec\phi)\approx \mathbf I$，则有：

$$
\begin{aligned}\operatorname{Exp}\left(\mathbf{c}_ {k}\right) \operatorname{Exp}\left(\mathbf{c}_ {k+1}\right)&=\operatorname{Exp}\left(\mathbf{c}_ {k}\right) \operatorname{Exp}\left(\mathbf{I}\cdot\mathbf{c}_ {k+1}\right)\&\approx \operatorname{Exp}\left(\mathbf{c}_ {k}\right) \operatorname{Exp}\left(\mathbf J_r(\mathbf c_k)\cdot\mathbf{c}_ {k+1}\right) \ &\approx \operatorname{Exp}\left(\mathbf{c}_ {k}+\mathbf{c}_ {k+1}\right)\end{aligned}
$$

因此有：

$$
\begin{aligned}\prod_ {k=i}^{j-1} \operatorname{Exp}\left(\mathbf{c}_ {k}\right) &=\operatorname{Exp}\left(\mathbf{c}_ {i}\right) \operatorname{Exp}\left(\mathbf{c}_ {i+1}\right) \prod_ {k=i+2}^{j-1} \operatorname{Exp}\left(\mathbf{c}_ {k}\right) \& \approx \operatorname{Exp}\left(\mathbf{c}_ {i}+\mathbf{c}_ {i+1}\right) \operatorname{Exp}\left(\mathbf{c}_ {i+2}\right) \prod_ {k=i+3}^{j-1} \operatorname{Exp}\left(\mathbf{c}_ {k}\right) \& \approx \operatorname{Exp}\left(\mathbf{c}_ {i}+\mathbf{c}_ {i+1}+\mathbf{c}_ {i+2}\right) \operatorname{Exp}\left(\mathbf{c}_ {i+3}\right) \prod_ {k=i+4}^{j-1} \operatorname{Exp}\left(\mathbf{c}_ {k}\right) \& \approx \cdots \approx \operatorname{Exp}\left(\mathbf{c}_ {i}+\mathbf{c}_ {i+1}+\mathbf{c}_ {i+2}+\cdots \mathbf{c}_ {j-1}\right) \&=\operatorname{Exp}\left(\sum_ {k=i}^{j-1} \mathbf{c}_ {k}\right)\end{aligned}
$$

所以可得：

$$
\Delta \hat{\mathbf{R}}_ {i j}\approx\Delta \overline{\mathbf{R}}_ {i j} \operatorname{Exp}\left(\sum_ {k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_ {k+1，j}^{T} \mathbf{J}_ {r}^{k} \delta \mathbf{b}_ {i}^{g} \Delta t\right)\right)
$$

即：

$$
\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_ {k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_ {k+1 ,j}^{T} \mathbf{J}_ {r}^{k} \Delta t\right)
$$

在实现时，可以进一步引出递推式：

$$
\begin{aligned}\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}&=\sum_ {k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_ {k+1 ,j}^{T} \mathbf{J}_ {r}^{k} \Delta t\right)\ &=\sum_ {k=i}^{j-2}\left(\left(-\Delta \overline{\mathbf{R}}_ {k+1 ,j-1}\cdot\Delta\overline{\mathbf{R}}_ {j-1, j}\right)^{T} \mathbf{J}_ {r}^{j} \Delta t\right) - \Delta \overline{\mathbf{R}}_ {j j}^T \mathbf{J}_ {r}^{j-1}\Delta t \&= \sum_ {k=i}^{j-2}\left(-\Delta \overline{\mathbf{R}}^T_ {j-1, j}\cdot\Delta\overline{\mathbf{R}}_ {k+1 ,j-1}^T \mathbf{J}_ {r}^{k} \Delta t\right) - \mathbf{J}_ {r}^{j-1}\Delta t\&= \Delta\overline{\mathbf{R}}^T_ {j-1, j}\cdot\sum_ {k=i}^{j-2}\left(-\Delta \overline{\mathbf{R}}_ {k+1 ,j-1}^{T} \mathbf{J}_ {r}^{k} \Delta t\right) - \mathbf{J}_ {r}^{j-1}\Delta t \&=\Delta\overline{\mathbf{R}}^T_ {j-1, j} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i, j-1}}{\partial \overline{\mathbf{b}}^{g}}- \mathbf{J}_ {r}^{j-1}\Delta t\&=\Delta\overline{\mathbf{R}}_ {j, j-1} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i, j-1}}{\partial \overline{\mathbf{b}}^{g}}- \mathbf{J}_ {r}^{j-1}\Delta t\end{aligned}
$$

（2）速度项（$\Delta \hat{\mathbf{v}}_ {i j} \approx \Delta \overline{\mathbf{v}}_ {i j}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}$）：

$$
\begin{aligned}\Delta \hat{\mathbf{v}}_ {i j} &=\Delta \tilde{\mathbf{v}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right) \&=\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\hat{\mathbf{b}}_ {i}^{a}\right) \Delta t\right] \& \approx \sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}-\delta \mathbf{b}_ {i}^{a}\right) \Delta t\right] \& \approx \sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}-\delta \mathbf{b}_ {i}^{a}\right) \Delta t\right] \&=\sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right) \Delta t-\Delta \overline{\mathbf{R}}_ {i k} \delta \mathbf{b}_ {i}^{a} \Delta t+\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)^{\wedge}\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right) \Delta t-\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)^{\wedge} \delta \mathbf{b}_ {i}^{a} \Delta t\right] \& \approx \sum_ {k=i}^{j-1}\Delta \tilde{\mathbf{R}}_ {i k} (\overline{\mathbf{b}}_ {i}^g)\cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right) \Delta t+\sum_ {k=i}^{j-1}\left{-\left[\Delta \overline{\mathbf{R}}_ {i k} \Delta t\right] \delta \mathbf{b}_ {i}^{a}-\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right] \delta \mathbf{b}_ {i}^{g}\right}\& = \Delta \overline{\mathbf{v}}_ {i j}+\sum_ {k=i}^{j-1}\left{-\left[\Delta \overline{\mathbf{R}}_ {i k} \Delta t\right] \delta \mathbf{b}_ {i}^{a}-\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right] \delta \mathbf{b}_ {i}^{g}\right}\end{aligned}
$$

上述中分别用到了一阶近似，$\text{Exp}(\delta\vec\phi) \approx(\delta\vec\phi)^\wedge$以及$\mathbf{a}^{\wedge} \cdot \mathbf{b}=-\mathbf{b}^{\wedge} \cdot \mathbf{a}$，且忽略了高阶小项$\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)^{\wedge} \delta \mathbf{b}_ {i}^{a}$。所以有：

$$
\begin{array}{l}\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}=-\sum_ {k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}}=-\sum_ {k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_ {i k} \Delta t\right)\end{array}
$$

（3）位置项（$\Delta \hat{\mathbf{p}}_ {i j} \approx \Delta \overline{\mathbf{p}}_ {i j}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}$）：

$$
\begin{aligned}\Delta \hat{\mathbf{p}}_ {i j} &=\Delta \tilde{\mathbf{p}}_ {i j}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right) \&=\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right) \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\hat{\mathbf{b}}_ {i}^{a}\right) \Delta t^{2}\right] \&=\underbrace{\sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}, \hat{\mathbf{b}}_ {i}^{a}\right) \Delta t\right]}_ {\langle 1\rangle}+\underbrace{\frac{1}{2} \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\hat{\mathbf{b}}_ {i}^{a}\right) \Delta t^{2}\right]}_ {\langle 2\rangle}\end{aligned}
$$

下面我们分别推导$\langle 1\rangle$和$\langle 2\rangle$部分。

$$
\begin{aligned}\langle 1\rangle &=\sum_ {k=i}^{j-1}\left[\left(\Delta \overline{\mathbf{v}}_ {i k}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}\right) \Delta t\right] \&=\sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{v}}_ {i k} \Delta t+\left(\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \delta \mathbf{b}_ {i}^{g}+\left(\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t\right) \delta \mathbf{b}_ {i}^{a}\right]\end{aligned}
$$

$$
\begin{aligned}\langle 2 \rangle & =\frac{\Delta t^{2}}{2} \sum_ {k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_ {i k}\left(\hat{\mathbf{b}}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\hat{\mathbf{b}}_ {i}^{a}\right)\right] \&=\frac{\Delta t^{2}}{2} \sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}-\delta \mathbf{b}_ {i}^{a}\right)\right] \& \approx \frac{\Delta t^{2}}{2} \sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}-\delta \mathbf{b}_ {i}^{a}\right)\right] \& \approx \frac{\Delta t^{2}}{2} \sum_ {k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)-\Delta \overline{\mathbf{R}}_ {i k} \delta \mathbf{b}_ {i}^{a}-\Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right]\end{aligned}
$$

$\langle2\rangle$的推导与速度项是非常类似的，也是忽略了高阶小项。将$\langle1\rangle, \langle2\rangle$合起来，可以得到：

$$
\begin{aligned}&\langle1\rangle+\langle2\rangle\&=\sum_ {k=i}^{j-1}\left{\left[\Delta \overline{\mathbf{v}}_ {i k} \Delta t+\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right) \Delta t^{2}\right]+\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \delta \mathbf{b}_ {i}^{g}+\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{\alpha}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \Delta t^{2}\right] \delta \mathbf{b}_ {i}^{a}\right}\&=\Delta \overline{\mathbf{p}}_ {i j}+\left{\sum_ {k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right]\right} \delta \mathbf{b}_ {i}^{g}+\left{\sum_ {k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \Delta t^{2}\right]\right} \delta \mathbf{b}_ {i}^{a}\end{aligned}
$$

所以有：

$$
\begin{array}{l}\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_ {k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \cdot\left(\tilde{\mathbf{f}}_ {k}-\overline{\mathbf{b}}_ {i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_ {i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}}=\sum_ {k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_ {i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_ {i k} \Delta t^{2}\right]\end{array}
$$

残差

之所以需要残差，是因为IMU预积分的测量值实际上也是优化中的一个约束量，pose graph中的边。而一般来说，预积分的残差是由计算值和测量值决定的，这里的计算值可能是通过视觉约束等等方法得到的。我们知道IMU预积分的理想值是如下：

$$
\begin{array}{l}\Delta \mathbf{R}_ {i j}=\mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \\Delta \mathbf{v}_ {i j}=\mathbf{R}_ {i}^{T}\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right) \\Delta \mathbf{p}_ {i j}=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)\end{array}
$$

其三项残差定义如下：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{R}_ {i j}} & \triangleq \text{Log} \left{\left[\Delta \tilde{\mathbf{R}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)\right]^{T} \cdot \mathbf{R}_ {i}^{T} \mathbf{R}_ {j}\right} \& \triangleq \text{Log} \left[\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \Delta \mathbf{R}_ {i j}\right] \& \triangleq \Delta \mathbf{v}_ {i j}-\Delta \hat{\mathbf{v}}_ {i j} \\mathbf{r}_ {\Delta \mathbf{v}_ {i j}} & \triangleq \mathbf{R}_ {i}^{T}\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\left[\Delta \tilde{\mathbf{v}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}\right] \\mathbf{r}_ {\Delta \mathbf{p}_ {ij}} & \triangleq \mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}, \overline{\mathbf{b}}_ {i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_ {i}^{a}\right] \& \triangleq \Delta \mathbf{p}_ {i j}-\Delta \hat{\mathbf{p}}_ {i j}\end{aligned}
$$

从定义上来看，残差也就是用来度量计算值与测量值的差别，这也是很合理的。从前面的推导可以知道，这个测量值是由噪声加上理想值得到的（实际上我们是从理想值中分离出了噪声）。

  • 一般来说，在SLAM系统中，我们需要优化的是$\mathbf{R}_ {i}, \mathbf{p}_ {i}, \mathbf{v}_ {i}, \mathbf{R}_ {j}, \mathbf{p}_ {j}, \mathbf{v}_ {j}$，不过在带有IMU的系统中，bias的影响往往不可忽视，因此一般也会加上对bias的优化。不过从残差的定义上来看，实际上优化的是bias的变化量$\delta \mathbf{b}_ {i}^{g}, \delta \mathbf{b}_ {i}^{a}$。所以在IMU预积分中，完整的一个导航状态为：$\mathbf{R}_ {i}, \mathbf{p}_ {i}, \mathbf{v}_ {i}, \mathbf{R}_ {j}, \mathbf{p}_ {j}, \mathbf{v}_ {j}, \delta \mathbf{b}_ {i}^{g}, \delta \mathbf{b}_ {i}^{a}$。

  • 对各个状态的更新操作如下：

    $$
    \begin{array}{l}\mathbf{R}_ {i} \leftarrow \mathbf{R}_ {i} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right) ; \mathbf{p}_ {i} \leftarrow \mathbf{p}_ {i}+\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i}; \mathbf{v}_ {i} \leftarrow \mathbf{v}_ {i}+\delta \mathbf{v}_ {i}; \\mathbf{R}_ {j} \leftarrow \mathbf{R}_ {j} \cdot \operatorname{Exp}\left(\delta \vec{\phi}_ {j}\right) ; \mathbf{p}_ {j} \leftarrow \mathbf{p}_ {j}+\mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j} ; \mathbf{v}_ {j} \leftarrow \mathbf{v}_ {j}+\delta \mathbf{v}_ {j}; \\delta \mathbf{b}_ {i}^{g} \leftarrow \delta \mathbf{b}_ {i}^{g}+\widetilde{\delta \mathbf{b}_ {i}^{g}} ; \delta \mathbf{b}_ {i}^{a} \leftarrow \delta \mathbf{b}_ {i}^{a}+\widetilde{\delta \mathbf{b}_ {i}^{a}}\end{array}
    $$

    需要注意的是，由于我们优化的是bias的增量${\delta \mathbf{b}_ {i}^{g}},{\delta \mathbf{b}_ {i}^{a}}$，因此这里的$\widetilde{\delta \mathbf{b}_ {i}^{g}},\widetilde{\delta \mathbf{b}_ {i}^{a}}$实际上是增量的增量，也就是通过增量的Jacobian来计算的。但是实际上增量的Jacobian与原来的Jacobian计算方法上是一样的，只不过它的初始状态是从0开始（实际上之前也运用过这样的方式进行优化过，每次优化的变量实际上是更新量）。

  • 为何位置没有采用速度一样的方式去更新（$\mathbf{p}_ {i} \leftarrow \mathbf{p}_ {i}+\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i}; \mathbf{v}_ {i} \leftarrow \mathbf{v}_ {i}+\delta \mathbf{v}_ {i}$）？这个问题实际上如果是在$\text{SE(3)}$上，也就是直接用transformation matrix，得到的结果是：

    $$
    \begin{aligned}\mathbf{T}_ {i}^{\prime} &=\mathbf{T}_ {i} \cdot \delta \mathbf{T}_ {i}=\left[\begin{array}{cc}\mathbf{R}_ {i} & \mathbf{p}_ {i} \0 & 1\end{array}\right]\left[\begin{array}{cc}\delta \mathbf{R}_ {i} & \delta \mathbf{p}_ {i} \0 & 1\end{array}\right] \&=\left[\begin{array}{cc}\mathbf{R}_ {i} \cdot \delta \mathbf{R}_ {i} & \mathbf{p}_ {i}+\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i} \0 & 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}_ {i}^{\prime} & \mathbf{p}_ {i}^{\prime} \0 & 1\end{array}\right]\end{aligned}
    $$

    也就是上述更新的操作，从物理意义上理解，就是先进行了旋转再进行了平移的变换。不过，毕竟我们一麻溜推导下来，用的都是$\text{SO(3)}$上的操作，平移与旋转是分开操作的（平移的残差中已经包含了旋转的计算），我个人认为还是$\mathbf{p}_ {i} \leftarrow \mathbf{p}_ {i}+ \delta \mathbf{p}_ {i}$理论上更合理一些。在实际中，可能二者使用起来没什么太大的区别。

  • 由于噪声不是的零均值正态分布，因此不同的协方差对应着不同约束项的权重（实际上权重是协方差的逆），这样给出来的结果是一个无偏估计。最终我们优化目标就是能使得加权残差的平方和最小。另外，在优化时候，实际上优化变量是由Jacobian矩阵来决定的（高斯牛顿的海森矩阵也是由Jacobian矩阵来近似的），因此我们的下一个任务就是求出Jacobian矩阵。

Jacobian

接下来是最后一步，计算出Jacobian矩阵。我们将Jacobian矩阵分成三类：（1）”0“矩阵，也就是残差和某些变量是完全无关的；（2）线性的，也就是残差和某些变量是线性相关的，这样的Jacobian也可以由系数直接得到；（3）其他的，也就是残差和某些变量的关系较为复杂，需要进行相应的变形才能求得Jacobian矩阵。

$\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}$的Jacobian

（1）“0”矩阵。由于$\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}$中不含$\mathbf{p}_ {i} , \mathbf{p}_ {j}, \mathbf{V}_ {i}, \mathbf{V}_ {j}, \delta \mathbf{b}_ {i}^{a}$，因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {ij}}}{\partial \mathbf{p}_ {i}}=\mathbf{0},\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {ij}}}{\partial \mathbf{p}_ {j}}=\mathbf{0},\\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}}{\partial \mathbf{v}_ {i}}=\mathbf{0},\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}}{\partial \mathbf{v}_ {j}}=\mathbf{0},\\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}}{\partial \delta \mathbf{b}_ {i}^{a}}=\mathbf{0}
$$

（2）线性类：无。

（3）其他类：

  • 下面求关于$\vec{\phi}_ {i}$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right) &=\text{Log} \left[\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T}\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right)^{T} \mathbf{R}_ {j}\right] \& =\text{Log} \left[\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \operatorname{Exp}\left(-\delta \vec{\phi}_ {i}\right) \mathbf{R}_ {i}^{T} \mathbf{R}_ {j}\right] \&=\text{Log}\left[\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \operatorname{Exp}\left(-\mathbf{R}_ {j}^{T} \mathbf{R}_ {i} \delta \vec{\phi}_ {i}\right)\right] \&=\text{Log} \left{\operatorname{Exp}\left[\text{Log}\left(\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \mathbf{R}_ {i}^{T} \mathbf{R}_ {j}\right)\right] \cdot \operatorname{Exp}\left(-\mathbf{R}_ {j}^{T} \mathbf{R}_ {i} \delta \vec{\phi}_ {i}\right)\right} \&=\text{Log}\left[\operatorname{Exp}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\mathbf{R}_ {i}\right)\right) \cdot \operatorname{Exp}\left(-\mathbf{R}_ {j}^{T} \mathbf{R}_ {i} \delta \vec{\phi}_ {i}\right)\right] \& \approx \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\mathbf{R}_ {i}\right)-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\mathbf{R}_ {i}\right)\right) \mathbf{R}_ {j}^{T} \mathbf{R}_ {i} \delta \vec{\phi}_ {i} \& = \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \mathbf{R}_ {j}^{T} \mathbf{R}_ {i} \delta \vec{\phi}_ {i}\end{aligned}
$$

上面的推导中，用到的性质有：$(\operatorname{Exp}(\vec{\phi}))^{T}=\operatorname{Exp}(-\vec{\phi})$，伴随性质，$\text{Log} (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))=\vec{\phi}+\mathbf{J}_ {r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}$。综上所述，有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}}{\partial \vec{\phi}_ {i}}=-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \mathbf{R}_ {j}^{T} \mathbf{R}_ {i}
$$

  • 使用类似的方法，可以得到$\vec{\phi}_ {j}$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta R_ {i j}}\left(\mathbf{R}_ {j} \operatorname{Exp}\left(\delta \vec{\phi}_ {j}\right)\right) &=\text{Log} \left[\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \operatorname{Exp}\left(\delta \vec{\phi}_ {j}\right)\right] \&=\text{Log} \left{\operatorname{Exp}\left[\text{Log} \left(\left(\Delta \hat{\mathbf{R}}_ {i j}\right)^{T} \mathbf{R}_ {i}^{T} \mathbf{R}_ {j}\right)\right] \cdot \operatorname{Exp}\left(\delta \vec{\phi}_ {j}\right)\right} \&=\text{Log} \left{\operatorname{Exp}\left(\mathbf{r}_ {\Delta R_ {i j}}\left(\mathbf{R}_ {j}\right)\right) \cdot \operatorname{Exp}\left(\delta \vec{\phi}_ {j}\right)\right} \& \approx \mathbf{r}_ {\Delta R_ {i j}}\left(\mathbf{R}_ {j}\right)+\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta R_ {i j}}\left(\mathbf{R}_ {j}\right)\right) \delta \vec{\phi}_ {j} \& = \mathbf{r}_ {\Delta R_ {i j}}+\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta R_ {i j}}\right) \delta \vec{\phi}_ {j}\end{aligned}
$$

因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}}{\partial \vec{\phi}_ {j}}=\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right)
$$

  • 关于$\delta \mathbf{b}_ {i}^g$的Jacobian（$\mathbf R_ {i}, \mathbf R_ {j}$都是与残差项$\Delta \mathbf{R}_ {ij}$有关，而$\delta \mathbf{b}_ {i}^g$则是与残差项$\Delta \tilde{\mathbf{R}}_ {ij}$有关）：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}+\widetilde{\delta \mathbf{b}_ {i}^{g}}\right) &\approx\text{Log}\left{\left[\Delta \tilde{\mathbf{R}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}\left(\delta \mathbf{b}_ {i}^{g}+\widetilde{\delta \mathbf{b}_ {i}^{g}}\right)\right)\right]^{T} \mathbf{R}_ {i}^{T} \mathbf{R}_ {j}\right} \& \approx \text{Log} \left{\left[\Delta \tilde{\mathbf{R}}_ {i j}\left(\overline{\mathbf{b}}_ {i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \operatorname{Exp}\left(\mathbf{J}_ {r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\right)\right]^{T} \Delta \mathbf{R}_ {i j}\right} \end{aligned}
$$

令$\boldsymbol{\varepsilon} \doteq \mathbf{J}_ {r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right)$，则：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}+\widetilde{\delta \mathbf{b}_ {i}^{g}}\right) &\approx \text{Log} \left{\left[\Delta \hat{\mathbf{R}}_ {i j} \cdot \operatorname{Exp}\left(\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{y}}\right)\right]^{T} \Delta \mathbf{R}_ {i j}\right}\&= \text{Log} \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\right) \Delta \hat{\mathbf{R}}_ {i j}^{T} \Delta \mathbf{R}_ {i j}\right]\&=\text{Log} \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\right) \operatorname{Exp}\left(\text{Log} \left(\Delta \hat{\mathbf{R}}_ {i j}^{T} \Delta \mathbf{R}_ {i j}\right)\right)\right]\&=\text{Log} \left[\operatorname{Exp}\left(-\boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\right) \operatorname{Exp}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)\right)\right]\&=\text{Log} \left{\operatorname{Exp}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)\right) \operatorname{Exp}\left[-\operatorname{Exp}\left(-\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)\right) \cdot \boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\right]\right}\&\approx \mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\left(\delta \mathbf{b}_ {i}^{g}\right)\right) \cdot \boldsymbol{\varepsilon} \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \widetilde{\delta \mathbf{b}_ {i}^{g}}\&=\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \cdot \mathbf{J}_ {r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \cdot \widetilde{\delta \mathbf{b}_ {i}^{g}}\end{aligned}
$$

上面的推导中，用到的性质有：$\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_ {r}(\vec{\phi}) \cdot \delta \vec{\phi}\right)$，$(\operatorname{Exp}(\vec{\phi}))^{T}=\operatorname{Exp}(-\vec{\phi})$，伴随性质，$\text{Log} (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))=\vec{\phi}+\mathbf{J}_ {r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}$（可以看出来推导的方法是很类似的）。综上所述，可以得到：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{R}_ {ij}}}{\partial \delta \mathbf{b}_ {i}^{\mathrm{g}}}=-\mathbf{J}_ {r}^{-1}\left(\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \cdot \operatorname{Exp}\left(-\mathbf{r}_ {\Delta \mathbf{R}_ {i j}}\right) \cdot \mathbf{J}_ {r}\left(\frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_ {i}^{g}\right) \cdot \frac{\partial \Delta \overline{\mathbf{R}}_ {i j}}{\partial \overline{\mathbf{b}}^{g}}
$$

这里需要注意的这儿我们是对$\delta \mathbf{b}_ {i}^{g}$求导，而$\mathbf{r}_ {\Delta \mathbf{R}_ {ij}}$也是新的bias（$\mathbf{b}_i^g+\delta \mathbf {b}_i^g$）的测量预积分值。至于为什么要这么做，也许之后会找到答案。

$\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}$的Jacobian

（1）“0”类：由于$\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}$中不包含$\mathbf{R}_ {j}, \mathbf{p}_ {i}, \mathbf{p}_ {j}$，因此：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {ij}}}{\partial \vec{\phi}_ {j}}=\mathbf{0}, \frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {ij}}}{\partial \mathbf{p}_ {i}}=\mathbf{0}, \frac{\partial \mathbf{r}_ {\Delta v_ {i j}}}{\partial \mathbf{p}_ {j}}=\mathbf{0}
$$

（2）线性类：由于$\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}$与$\delta \mathbf{b}_ {i}^{g} , \delta \mathbf{b}_ {i}^{a}$是线性关系，因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {i j}}}{\partial \delta \mathbf{b}_ {i}^{\mathrm{g}}}=-\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \mathbf{b}^{g}}, \frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {j j}}}{\partial \delta \mathbf{b}_ {i}^{\mathrm{a}}}=-\frac{\partial \Delta \overline{\mathbf{v}}_ {i j}}{\partial \mathbf{b}^{a}}
$$

（3）其他类：

  • 关于$\mathbf{v}_i$的Jacobian

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{v}_ {j j}}\left(\mathbf{v}_ {i}+\delta \mathbf{v}_ {i}\right) &=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\delta \mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j} \&=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j}-\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {i} \&=\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}\left(\mathbf{v}_ {i}\right)-\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {i} \ &=\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}-\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {i}\end{aligned}
$$

因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {i j}}}{\partial \mathbf{v}_ {i}}=-\mathbf{R}_ {i}^{T}
$$

  • 关于$\mathbf{v}_j$的Jacobian

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{v}_ {y}}\left(\mathbf{v}_ {j}+\delta \mathbf{v}_ {j}\right) &=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}+\delta \mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j} \&=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j}+\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {j} \&=\mathbf{r}_ {\Delta \mathbf{v}_ {i j}}\left(\mathbf{v}_ {j}\right)+\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {j} \&= \mathbf{r}_ {\Delta \mathbf{v}_ {j j}}+\mathbf{R}_ {i}^{T} \delta \mathbf{v}_ {j}\end{aligned}
$$

因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {i j}}}{\partial \mathbf{v}_ {j}}=\mathbf{R}_ {i}^{T}
$$

  • 关于$\vec\phi_i$的Jacobian

$$
\begin{aligned}\mathbf{r}_ {\Delta v_ {ij}}\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right) &=\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right)^{T}\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j} \& = \operatorname{Exp}\left(-\delta \vec{\phi}_ {i}\right) \cdot \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j} \& \approx\left(\mathbf{I}-\left(\delta \vec{\phi}_ {i}\right)^{\wedge}\right) \cdot \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j} \&=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)-\Delta \hat{\mathbf{v}}_ {i j}-\left(\delta \vec{\phi}_ {i}\right)^{\wedge} \cdot \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right) \& =\mathbf{r}_ {\Delta v_ {i j}}\left(\mathbf{R}_ {i}\right)+\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_ {i} \& = \mathbf{r}_ {\Delta v_ {i j}}+\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_ {i}\end{aligned}
$$

上述推导用到的规则有：$(\operatorname{Exp}(\vec{\phi}))^{T}=\operatorname{Exp}(-\vec{\phi})$，$\operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right) \approx \mathbf{I}+(\delta\vec\phi_i)^{\wedge}$，$\mathbf{a}^{\wedge} \cdot \mathbf{b}=-\mathbf{b}^{\wedge} \cdot \mathbf{a}$。因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{v}_ {i j}}}{\partial \vec{\phi}_ {i}}=\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{v}_ {j}-\mathbf{v}_ {i}-\mathbf{g} \cdot \Delta t_ {i j}\right)\right]^{\wedge}
$$

$\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}$的Jacobian

（1）“0”类：由于$\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}$中不包含$\mathbf R_ {j}, \mathbf{v}_j$

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {j j}}}{\partial \vec{\phi}_ {j}}=\mathbf{0}, \frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \mathbf{v}_ {j}}=\mathbf{0}
$$

（2）线性类：由于$\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}$与$\delta \mathbf{b}_ {i}^{g} , \delta \mathbf{b}_ {i}^{a}$是线性关系，因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \delta \mathbf{b}_ {i}^{g}}=-\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \mathbf{b}^{g}}, \frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \delta \mathbf{b}_ {i}^{a}}=-\frac{\partial \Delta \overline{\mathbf{p}}_ {i j}}{\partial \mathbf{b}^{a}}
$$

（3）其他类：

  • 关于$\mathbf{p}_i$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}\left(\mathbf{p}_ {i}+\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i}\right) &=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \&=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j}-\mathbf{I} \cdot \delta \mathbf{p}_ {i} \&=\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}\left(\mathbf{p}_ {i}\right)-\mathbf{I} \cdot \delta \mathbf{p}_ {i} \&=\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}-\mathbf{I} \cdot \delta \mathbf{p}_ {i}\end{aligned}
$$

因此可以得到：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \mathbf{p}_ {i}}=-\mathbf{I}
$$

  • 关于$\mathbf{p}_j$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}\left(\mathbf{p}_ {j}+\mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j}\right) &=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}+\mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \&=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j}+\mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j} \&=\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}\left(\mathbf{p}_ {j}\right)+\mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j} \&=\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}+\mathbf{R}_ {i}^{T} \mathbf{R}_ {j} \cdot \delta \mathbf{p}_ {j}\end{aligned}
$$

因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \mathbf{p}_ {j}}=\mathbf{R}_ {i}^{T} \mathbf{R}_ {j}
$$

  • 关于$\mathbf{v}_j$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}\left(\mathbf{v}_ {i}+\delta \mathbf{v}_ {i}\right) &=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\delta \mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \&=\mathbf{R}_ {i}^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j}-\mathbf{R}_ {i}^{T} \Delta t_ {i j} \cdot \delta \mathbf{v}_ {i} \&=\mathbf{r}_ {\Delta \mathbf{p}_ {ij}}\left(\mathbf{v}_ {i}\right)-\mathbf{R}_ {i}^{T} \Delta t_ {i j} \cdot \delta \mathbf{v}_ {i} \ &=\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}-\mathbf{R}_ {i}^{T} \Delta t_ {i j} \cdot \delta \mathbf{v}_ {i}\end{aligned}
$$

因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \mathbf{v}_ {i}}=-\mathbf{R}_ {i}^{T} \Delta t_ {i j}
$$

  • 关于$\vec\phi_i$的Jacobian：

$$
\begin{aligned}\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right) &=\left(\mathbf{R}_ {i} \operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right)\right)^{T}\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \& = \operatorname{Exp}\left(-\delta \vec{\phi}_ {i}\right) \cdot \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \& \approx\left(\mathbf{I}-\left(\delta \vec{\phi}_ {i}\right)^{\wedge}\right) \cdot \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j} \&=\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)-\Delta \hat{\mathbf{p}}_ {i j}-\left(\delta \vec{\phi}_ {i}\right)^{\wedge} \mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right) \& = \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}\left(\mathbf{R}_ {i}\right)+\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_ {i} \& =\mathbf{r}_ {\Delta \mathbf{p}_ {i j}}+\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)\right]^{\wedge} \cdot \delta \vec{\phi}_ {i}\end{aligned}
$$

上述推导用到的规则有：$(\operatorname{Exp}(\vec{\phi}))^{T}=\operatorname{Exp}(-\vec{\phi})$，$\operatorname{Exp}\left(\delta \vec{\phi}_ {i}\right) \approx \mathbf{I}+(\delta\vec\phi_i)^{\wedge}$，$\mathbf{a}^{\wedge} \cdot \mathbf{b}=-\mathbf{b}^{\wedge} \cdot \mathbf{a}$。因此有：

$$
\frac{\partial \mathbf{r}_ {\Delta \mathbf{p}_ {i j}}}{\partial \ \vec{\phi}_ {i}}=\left[\mathbf{R}_ {i}^{T} \cdot\left(\mathbf{p}_ {j}-\mathbf{p}_ {i}-\mathbf{v}_ {i} \cdot \Delta t_ {i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_ {i j}^{2}\right)\right]^{\wedge}
$$

注意，由于位置的更新方式是$\mathbf{p}_ {i} \leftarrow \mathbf{p}_ {i}+\mathbf{R}_ {i} \cdot \delta \mathbf{p}_ {i}$，因此我们在推导的时候也是加上了这个来求Jacobian。如果换了$\mathbf{p}_ {i} \leftarrow \mathbf{p}_ {i}+ \delta \mathbf{p}_ {i}$的更新方式，实际上结果也一样根据上面的方法很容易求出来。

至此，全文将 Foster 的 IMU 预积分理论中的公式推导结束，感谢邱笑晨提供的PDF。

Note: 在Vins mono中，IMU的重力也会被优化。