基于中值积分的IMU预积分公式推导

基于中值积分的IMU预积分公式推导

最近一直在准备毕设，需要用到imu预积分的知识，大概把vins的代码都看完了，vins中预积分的部分使用的是欧拉积分，而大多数文献中给出的推导过程都是基于欧拉积分，本着造轮子的心态，花了一周时间自己推导了一下中值积分的公式，现在分享给大家，由于本人刚刚接触VIO时间仅半年时间，推导难免有错误，欢迎大家指正。

本人微信号：gch86869
欢迎交流

完整版公式推导pdf下载：
下载地址
提取码：8uro

数学基础

imu预积分一般是基于李代数的扰动模型，也就是so3的右扰动模型（左扰动和右扰动一样，只是施加扰动的位置不同）
下面给出so3常用公式以及右扰动模型的数学表达，但是省略推导过程，如果对基础李代数公式推导感兴趣，建议阅读高博的slam14讲或者机器人学中的状态估计。

1. 当 $\delta \phi$小量时，$Exp{\left(\left[\delta \phi \right]\right)}^{\wedge }\approx I+{\left[\delta \phi \right]}^{\wedge }$
2. ${\left[R\cdot \phi \right]}^{\wedge }=R\cdot {\left[\phi \right]}^{\wedge }\cdot R^T$
3. $Exp{\left({\left[\phi \right]}^{\wedge }\right)}^T=Exp\left({\left[-\phi \right]}^{\wedge }\right)$
4. $Exp\left({\left[\phi \right]}^{\wedge }\right)\cdot R=R\cdot Exp\left({\left[R^T\cdot \phi \right]}^{\wedge }\right)$
5. 当$\delta {\phi }_2$为小量时，$Log{\left(Exp\left({\left[{\phi }_1\right]}^{\wedge }\right)\cdot Exp\left({\left[{\phi }_2\right]}^{\wedge }\right)\right)}^{\vee }={\phi }_1+J_r^{-1}\left({\phi }_1\right)\cdot {\phi }_2$

手打了几个公式好辛苦啊，基本上imu预积分就是这个公式之间变来变去，基本上优先使用第一个公式，第五个公式不到万不得已不要用，因为求so3的右雅可比在计算机中还是蛮消耗计算量的，而且还需要引入额外的李代数的库，其他的操作公式1-4都可以直接用Eigen手打。

公式推导

我把imu预积分的公式推导分为三个部分，第一部分为imu预积分噪声的传播方程推导，第二部分为imu预积分残差对状态量的雅可比，第三部分为逆深度重投影残差对状态量（pose，landmark）的推导。
同时在推导雅可比时，还参照邱笑晨博士基于欧拉积分预积分推导里对雅可比的分类，0类，线性类，复杂类；让整个文件显得稍微有条理一点。

由于篇幅有限，这里只放出部分推导，其余的推导过程见附件。

一、IMU预积分与噪声传播方程

1.1 IMU 预积分定义

欧拉积分：

角速度测量值（世界坐标系）：
$\tilde{\varpi }={\varpi }_k-b_{g,k}+{\eta }_{g,k}$
加速度测量值（世界坐标系）：
$\tilde{a}=\Delta R_{i,k}\cdot \left(a_k-b_{a,k}+{\eta }_{a,k}\right)$
旋转预积分量：
$\Delta R_{i,k+1}=\Delta R_{i,k}\cdot Exp\left({\left[\frac{\Delta t}{2}\cdot \left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)\right]}^{\wedge }\right)$
速度预积分量：
$\Delta V_{i,k+1}=\Delta R_{i,k}\left(a_k-b_{g,i}+{\eta }_{a,k}\right)\Delta t+\Delta V_{i,k}$
位置（平移）预积分量：
$\Delta P_{i,k+1}=\Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{1}{2}\Delta R_{i,k}\cdot \left(a_k-b_{g,i}+{\eta }_{a,k}\right)\Delta t^2$
中值积分：
角速度测量值（世界坐标系）：
$\tilde{\varpi }=\frac{1}{2}\left[\left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)+\left({\varpi }_{k+1}-b_{g,k+1}+{\eta }_{g,k+1}\right)\right]$
加速度测量值（世界坐标系）：
$\tilde{a}=\frac{1}{2}\left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}+{\eta }_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(a_{k+1}-b_{a,k+1}+{\eta }_{a,k+1}\right)\right]$
旋转预积分量：
$\begin{array}{c}\Delta R_{i,k+1}=\Delta R_{i,k}\cdot Exp\left({\left[\frac{\Delta t}{2}\cdot \left(\left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)+\left({\varpi }_{k+1}-b_{g,k}+{\eta }_{g,k+1}\right)\right)\right]}^{\wedge }\right)\\ Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)=Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\frac{\Delta t}{2}\cdot \left(\left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)+\left({\varpi }_{k+1}-b_{g,k}+{\eta }_{g,k+1}\right)\right)\right]}^{\wedge }\right)\end{array}$
速度预积分量：
$\Delta V_{i,k+1}=\Delta V_{i,k}+\frac{\Delta t}{2}\cdot \left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}+{\eta }_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(a_{k+1}-b_{a,k}+{\eta }_{a,k+1}\right)\right]$
位置（平移）预积分量：
$\Delta P_{i,k+1}=\Delta P_{i,k+1}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}+{\eta }_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(a_{k+1}-b_{a,k}+{\eta }_{a,k+1}\right)\right]$
陀螺仪bias：
$b_{g,k+1}=b_{g,k}+{\eta }_{g,k}\cdot \Delta t$
加速度计bias：
$b_{a,k+1}=b_{a,k}+{\eta }_{a,k}\cdot \Delta t$
根据一阶泰勒展开得到噪声传播方程：
$\left[\begin{array}{c}\delta {\varphi }_{i,k+1}\\ \delta V_{i,k+1}\\ \delta P_{i,k+1}\\ \delta b_{g,k+1}\\ \delta b_{a,k+1}\end{array}\right]=F\cdot \left[\begin{array}{c}\delta {\varphi }_{i,k}\\ \delta V_{i,k}\\ \delta P_{i,k}\\ \delta b_{g,k}\\ \delta b_{a,k}\end{array}\right]+G\cdot \left[\begin{array}{c}{\eta }_{g,k}\\ {\eta }_{a,k}\\ {\eta }_{g,k+1}\\ {\eta }_{a,k+1}\\ {\eta }_{b_w,k}\\ {\eta }_{b_w,k}\end{array}\right]$
其中：
$F=\left[\begin{array}{ccccc}\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta {\varphi }_{i,k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta \Delta V_{i,k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta P_{i,k+1}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta b_{g,k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta b_{a,k}}\\ \frac{\partial \delta V_{i,k+1}}{\partial \delta {\varphi }_{i,k}}& \frac{\partial \delta V_{i,k+1}}{\partial \delta V_{i,k}}& \frac{\partial \delta V_{i,k+1}}{\partial \delta P_{i,k+1}}& \frac{\partial \delta V_{i,k+1}}{\partial \delta b_{g,k}}& \frac{\partial \delta V_{i,k+1}}{\partial \delta b_{a,k}}\\ \frac{\partial \delta P_{i,k+1}}{\partial \delta {\varphi }_{i,k}}& \frac{\partial \delta P_{i,k+1}}{\partial \delta V_{i,k}}& \frac{\partial \delta P_{i,k+1}}{\partial \delta P_{i,k+1}}& \frac{\partial \delta P_{i,k+1}}{\partial \delta b_{g,k}}& \frac{\partial \delta P_{i,k+1}}{\partial \delta b_{a,k}}\\ \frac{\partial \delta b_{g,k+1}}{\partial \delta {\varphi }_{i,k}}& \frac{\partial \delta b_{g,k+1}}{\partial \delta V_{i,k}}& \frac{\partial \delta b_{g,k+1}}{\partial \delta P_{i,k+1}}& \frac{\partial \delta b_{g,k+1}}{\partial \delta b_{g,k}}& \frac{\partial \delta b_{g,k+1}}{\partial \delta b_{a,k}}\\ \frac{\partial \delta b_{a,k+1}}{\partial \delta {\varphi }_{i,k}}& \frac{\partial \delta b_{a,k+1}}{\partial \delta V_{i,k}}& \frac{\partial \delta b_{a,k+1}}{\partial \delta P_{i,k+1}}& \frac{\partial \delta b_{a,k+1}}{\partial \delta b_{g,k}}& \frac{\partial \delta b_{a,k+1}}{\partial \delta b_{a,k}}\end{array}\right]$
$G=\left[\begin{array}{cccccc}\frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{g,k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{a,k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{g,k+1}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{a,k+1}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{b_{w,g},k}}& \frac{\partial \delta {\varphi }_{i,k+1}}{\partial {\eta }_{{}_{b_{w,a},k}}}\\ \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{g,k}}& \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{a,k}}& \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{g,k+1}}& \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{a,k+1}}& \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{b_{w,g},k}}& \frac{\partial \delta V_{i,k+1}}{\partial {\eta }_{{}_{b_{w,a},k}}}\\ \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{g,k}}& \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{a,k}}& \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{g,k+1}}& \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{a,k+1}}& \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{b_{w,g},k}}& \frac{\partial \delta P_{i,k+1}}{\partial {\eta }_{{}_{b_{w,a},k}}}\\ \frac{\partial b_{g,k+1}}{\partial {\eta }_{g,k}}& \frac{\partial \delta b_{g,k+1}}{\partial {\eta }_{a,k}}& \frac{\partial \delta b_{g,k+1}}{\partial {\eta }_{g,k+1}}& \frac{\partial \delta b_{g,k+1}}{\partial {\eta }_{a,k+1}}& \frac{\partial \delta b_{g,k+1}}{\partial {\eta }_{b_{w,g},k}}& \frac{\partial \delta b_{g,k+1}}{\partial {\eta }_{{}_{b_{w,a},k}}}\\ \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{g,k}}& \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{a,k}}& \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{g,k+1}}& \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{a,k+1}}& \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{b_{w,g},k}}& \frac{\partial \delta b_{a,k+1}}{\partial {\eta }_{{}_{b_{w,a},k}}}\end{array}\right]$
$\begin{array}{l}F\in R^{15\times 15}\\ G\in R^{15\times 18}\end{array}$
F为k+1时刻的状态量对k时刻状态量的雅可比
G为k+1时刻的状态量对噪声的雅可比
下面将雅可比分为三类：
①　0类
②　线性类
③　复杂类

1.2 对k时刻状态量噪声的雅可比（F）推导

1.2.1 k+1时刻旋转噪声的雅可比

$\begin{array}{c}\Delta R_{i,k+1}=\Delta R_{i,k}\cdot Exp\left({\left[\frac{\Delta t}{2}\cdot \left[\left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)+\left({\varpi }_{k+1}-b_{g,k}+{\eta }_{g,k+1}\right)\right]\right]}^{\wedge }\right)\\ Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)=Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\frac{\Delta t}{2}\cdot \left[\left({\varpi }_k-b_{g,k}+{\eta }_{g,k}\right)+\left({\varpi }_{k+1}-b_{g,k}+{\eta }_{g,k+1}\right)\right]\right]}^{\wedge }\right)\end{array}$
①0类:
$\delta {\varphi }_{i,k+1}$对$\delta P_{i,k}$、$\delta V_{i,k}$、$\delta b_{a,k}$的雅可比为0，既:
$\begin{array}{l}{F}_{12}=\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta V_{i,k}}=0\\ F_{13}=\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta V_{i,k}}=0\\ F_{15}=\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta b_{a,k}}=0\end{array}$
②线性类：
对$\delta b_{i,k}$的雅可比：
$\begin{array}{c}Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)=Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot Exp\left({\left[-\delta b_{g,k}\cdot \Delta t\right]}^{\wedge }\right)\\ Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)=Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot Exp\left({\left[-\delta b_{g,k}\cdot \Delta t\right]}^{\wedge }\right)\\ \delta {\varphi }_{i,k+1}=-\delta b_{g,k}\cdot \Delta t\\ F_{14}=\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta b_{g,k}}=-\Delta t\cdot I\end{array}$
③复杂类:
对$\delta {\varphi }_{i,k}$的雅可比：
$Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)=Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)$
左边：
$Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\approx Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot \left(I+{\left[\delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)$
其中：
当 $\delta \varphi$ 为小量时， $Exp\left({\left[\delta \varphi \right]}^{\wedge }\right)\approx I+{\left[\delta \varphi \right]}^{\wedge }$
右边：
$\begin{array}{c}Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\approx Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot Exp\left({\left[Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\left[\delta {\varphi }_{i,k}\right]\right]}^{\wedge }\right)\\ =Exp\left({\left[\Delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(I+{\left[Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\left[\delta {\varphi }_{i,k}\right]\right]}^{\wedge }\right)\\ =Exp\left({\left[\Delta {\varphi }_{i,k+1}\right]}^{\wedge }\right)\cdot \left(I+{\left[Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left[\delta {\varphi }_{i,k}\right]\right]}^{\wedge }\right)\end{array}$
其中：
$\begin{array}{c}Exp\left({\varphi }^{\wedge }\right)\cdot R=R\cdot Exp\left({\left[R^T\cdot \varphi \right]}^{\wedge }\right)\\ Exp{\left({\phi }^{\wedge }\right)}^T=Exp\left({\left[-\phi \right]}^{\wedge }\right)\end{array}$
所以：
$\begin{array}{c}\delta {\varphi }_{i,k+1}\approx Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \delta {\varphi }_{i,k}\\ \approx \left(I-{\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \delta {\varphi }_{i,k}\end{array}$
$F_{11}=\frac{\partial \delta {\varphi }_{i,k+1}}{\partial \delta {\varphi }_{i,k}}=I-{\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }$

1.2.3 k+1时刻位置噪声的雅可比

$\Delta P_{i,k+1}=\Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}+{\eta }_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(a_{k+1}-b_{a,k}+{\eta }_{a,k+1}\right)\right]$
②线性类：
对$\delta P_{i,k}$的雅可比：
$F_{33}=\frac{\partial \delta P_{i,k+1}}{\partial \delta P_{i,k}}=I$
对$\delta V_{i,k}$的雅可比：
$F_{32}=\frac{\partial \delta P_{i,k+1}}{\partial \delta V_{i,k}}=\Delta t\cdot I$
对$\delta b_{a,k}$的雅可比：
$\begin{array}{c}\Delta P_{i,k+1}+\delta P_{i,k+1}=\Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}-\delta b_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(a_{k+1}-b_{a,k}-\delta b_{a,k}\right)\right]\\ \delta P_{i,k+1}=-\frac{\Delta t^2}{4}\cdot \left(\Delta R_{i,k}\cdot \delta b_{a,k}+\Delta R_{i,k+1}\cdot \delta b_{a,k}\right)\\ F_{35}=\frac{\partial \delta P_{i,k+1}}{\delta b_{a,k}}=-\frac{\Delta t^2}{4}\left(\Delta R_{i,k}+\Delta R_{i,k+1}\right)\end{array}$
③复杂类：

对$\delta {\varphi }_{i,k}$的雅可比：
$\begin{array}{c}\Delta P_{i,k+1}+\delta P_{i,k+1}=\Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot Exp\left({\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot \left(a_k-b_{a,k}\right)+\Delta R_{i,k}\cdot Exp\left({\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]\\ \Delta P_{i,k+1}+\delta P_{i,k+1}\approx \Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot \left(I+{\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot \left(a_k-b_{a,k}\right)+\Delta R_{i,k}\cdot \left(I+{\left[\delta {\varphi }_{i,k}\right]}^{\wedge }\right)\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]\end{array}$
$\begin{array}{c}\delta P_{i,k+1}=\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot {\left(\delta {\varphi }_{i,k}\right)}^{\wedge }\cdot \left(a_k-b_{a,k}\right)+\Delta R_{i,k}\cdot {\left(\delta {\varphi }_{i,k}\right)}^{\wedge }\cdot Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]\\ \delta P_{i,k+1}=-\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot {\left(a_k-b_{a,k}\right)}^{\wedge }\cdot \delta {\varphi }_{i,k}+\Delta R_{i,k}\cdot {\left[Exp\left({\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]}^{\wedge }\cdot \delta {\varphi }_{i,k}\right]\\ \delta P_{i,k+1}=-\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot {\left(a_k-b_{a,k}\right)}^{\wedge }\cdot \delta {\varphi }_{i,k}+\Delta R_{i,k}\cdot Exp\left({\left[\tilde{\varpi }\Delta t\right]}^{\wedge }\right)\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\cdot Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \delta {\varphi }_{i,k}\right]\\ \delta P_{i,k+1}=-\frac{\Delta t^2}{4}\left[\Delta R_{i,k}\cdot {\left(a_k-b_{a,k}\right)}^{\wedge }\cdot \delta {\varphi }_{i,k}+\Delta R_{i,k+1}\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\cdot Exp\left({\left[-\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \delta {\varphi }_{i,k}\right]\\ \delta P_{i,k+1}\approx -\frac{\Delta t^2}{4}\left[\Delta R_{i,k}\cdot {\left(a_k-b_{a,k}\right)}^{\wedge }\cdot \delta {\varphi }_{i,k}+\Delta R_{i,k+1}\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\cdot \left(I-{\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\cdot \delta {\varphi }_{i,k}\right]\\ F_{31}=\frac{\partial \delta P_{i,k+1}}{\delta {\varphi }_{i,k}}=-\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot {\left(a_k-b_{a,k}\right)}^{\wedge }+\Delta R_{i,k+1}\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\cdot \left(I-{\left[\tilde{\varpi }\cdot \Delta t\right]}^{\wedge }\right)\right]\end{array}$

对$\delta b_{g,k}$的雅可比：
$\begin{array}{c}\Delta P_{i,k+1}+\delta P_{i,k+1}=\Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}\right)+\Delta R_{i,k+1}\cdot Exp\left({\left[-\delta b_{g,k}\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]\\ \Delta P_{i,k+1}+\delta P_{i,k+1}\approx \Delta P_{i,k}+\Delta V_{i,k}\cdot \Delta t+\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k}\cdot \left(a_k-b_{a,k}\right)+\Delta R_{i,k+1}\cdot \left(I-{\left[\delta b_{g,k}\cdot \Delta t\right]}^{\wedge }\right)\cdot \left(a_{k+1}-b_{a,k}\right)\right]\end{array}$
$\begin{array}{c}\delta P_{i,k+1}=-\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k+1}\cdot {\left(\delta b_{g,k}\cdot \Delta t\right)}^{\wedge }\cdot \left(a_{k+1}-b_{a,k}\right)\right]\\ \delta P_{i,k+1}=\frac{\Delta t^2}{4}\cdot \left[\Delta R_{i,k+1}\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\cdot \delta b_{g,k}\cdot \Delta t\right]\\ F_{34}=\frac{\partial \delta P_{i,k+1}}{\partial \delta b_{g,k}}=\frac{\Delta t^3}{4}\cdot \left[\Delta R_{i,k+1}\cdot {\left(a_{k+1}-b_{a,k}\right)}^{\wedge }\right]\end{array}$
[1]: 视觉slam14讲
[2]: 机器人学中的状态估计
[3]: 邱笑晨博士imu预积分推导