VINS-mono之IMU预积分（IMU连续时间积分和四元数求导）

VINS-Mono是HKUST的Shen Shaojie团队开源的一套非常优秀的Visual-Inertial融合定位算法。关于算法的介绍以及论文概览可以看此链接。
其开源代码可以从此地址下载，如果上github太慢，可以在我的博客此地址免费下载。

1 IMU运动模型

VINS-mono中的旋转表示是用四元数来表示的，所以整个系统会涉及到四元数求导的内容。
首先来看IMU的运动模型：

一般，我们只考虑IMU受高斯白噪声（n）和零偏（b）的影响。这里应该是设g^w=[0,0,-9.81]^T，所以用的加号。等式左边是IMU的测量值，a_t和w_t是真实值。

注意：这里的测量值当然都是处于IMU的坐标系上的。

2 PVQ 连续时间积分

我们可以对IMU的测量值进行积分得到IMU的位置、速度和旋转，即PVQ。在此先列出其连续时间积分公式。

注意：这里的PVQ都是处于世界坐标系下的。

可以看到前两项中，对加速度进行一次积分可以求得速度，对加速度求解两次积分可以得到位置。旋转是怎么来的呢，他当然是对四元数旋转的导数进行积分的来的，从上式可以看出他是一个包含角速度测量值的式子。
下面我们来具体推导。

3 四元数求导

对四元数求导之前，我们先推导一下四元数乘法的矩阵形式。
第一步，根据四元数乘法运算法则，算出两个四元数相乘得到的四元数向量。

从上面推导得出：

接下来将其写成矩阵与向量相乘的形式：

整理一下就是下面这种形式：

进一步简化，不难推出，

即，

好了，可以进行四元数求导了，具体推导过程如下：

知道四元数的导数之后就可以结合IMU运动模型理解连续时间IMU预积分了。
再次将IMU连续时间的预积分贴到这。

4 非常感谢您的阅读！