VSLAM:IMU预积分公式推导
一、IMU预积分
传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都是估计值,并且会不断调整,每次调整都会重新进行积分,传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合,避免优化时重复进行积分。四元数的表示方法有两种:一种是Hamilton(右手系)表示,另一种是JPL(左手系)表示。读者对公式推导时一定注意。
常用性质:
1. 四元数的连续积分:
2. 四元数的左乘与右乘:
我们将四元数表示为:
,则左右乘可以表示为:
3. 四元数导数如下:
有角速度:
,则导数可表示为:
1.1 当前时刻的位置,速度,旋转变量的连续表达式
我们将图像帧记作
及
,body坐标系下记作
和
,我们将位置,速度和旋转在时间
到
内进行积分,其世界坐标系下的公式可以写为:
1.2 当前时刻的位置,速度,旋转变量的离散表达式
我们以中值积分给出离散表示:
1.3 两帧之间的位置,速度,旋转增量的连续表达式
基本思想就是将参考坐标系从
转到第
帧的body坐标系下,相当于两边同时乘
,我们直接用论文中的公式来表示:
其中:
上述可以理解为
对
的相对运动量,其中
状态的改变并不会对其产生影响,可以将其作为非线性优化变量,避免重复计算。实际到这里,只要求解出积分,我们就完成了预积分的计算,我们的目标也就是在此。实际当中随机游走也是发生改变的,所以我们将上述变量再次进行一阶近似,我们再次使用论文中的公式进行表示:
至此,IMU的预积分表达式我们就已经得到了。
1.4 两帧之间的位置,速度,旋转增量的离散表达式
1.5 连续表达式下的位置、速度、旋转增量误差、协方差、Jacobian
IMU在每一个时刻积分出来的变量都是有误差的,我们针对误差进行分析,分析误差的传递方程,下面为连续时间下的误差导数方程:
我们对
进行推导,假设true表示真实测量值,含误差,nominal表示不含噪声的理论值,则有:
其中:
则:
我们再对
进行推导:
根据四元数的导数性质:
我们将上面的等式进行汇总:
最终:
我们将导数的定义拿来:
则:
对比扩展卡尔曼公式,我们发现其正好是非线性系统进行线性的表达式,据此,我们给出相似的协方差预测公式:
其中,协方差初始值为0,噪声协方差矩阵可以表示为:
类似,我们也可以获得误差的Jacobian迭代公式:
Jacobian的初始值为单位矩阵。
1.6 离散形式的增量分析
实际只需根据中值积分,将连续形式表达式进行离散化即可,推导过程省略,比较简单
其中:
离散误差传递方程可以简写为:
则Jacobian的迭代公式为:
Jacobian的初始值为:
。注意,我们在此计算Jacobian,仅仅是为了后端非线性优化过程对bias的计算提供帮助。
协方差的迭代公式:
协方差矩阵初始值为0,噪声的协方差矩阵为: