VINS理论与代码详解3——IMU预积分

VINS理论与代码详解3——IMU预积分

二．Vins_estimate文件夹中

         下面到第二部分了，也是整个框架的重点和难点，估计要花很长的时间学习和总计理论知识和代码逻辑，这一部分还是从论文的理论知识讲起，然后再附上代码消化理解。      

         论文内容：

1.      论文第IV点的B部分IMU预积分，

    IMU预积分的作用是计算出IMU数据的观测值以及其协方差矩阵，为后面的联合初始化提供初值以及后端优化提供IMU的约束关系。原始陀螺仪和加速度计的观测值数据：

    第一个式子等式左边是加速度测量值（你可以从加速度计中读到的值），等式右边是加速度真实值（其实就是准确的值，我们需要得到的是这个真实值）加上加速度计的偏置、重力加速度和加速度噪声项。第二个式子等式左边是陀螺仪测量值，等式右边是陀螺仪真实值加上陀螺仪偏置和陀螺仪噪声项。这里的值都是IMU（body）帧坐标系下的。这里假设噪声是服从高斯正态分布，而偏置服从随机游走模型。

     由上面最原始的式子积分就可以计算出下一时刻的p、v和q:

    这里等式左边的值都是世界坐标系下（W）bk+1帧的值。从整个式子可以看出来这里的状态传播需要bk帧下的旋转，平移和速度，当这些开始的状态发生改变的时候，就需要重新传播IMU观测值，也就是说状态传播方程要重新计算和修改，我们想要一次性就求出bk和bk+1之间的状态传播，因此选用预积分模型（其实这里我也没有完全搞明白，但是有一点是明白的，这里是在世界坐标系下求解状态，但是由条件里需要世界坐标系下的旋转，明显冲突啊，因此可以使用预积分将世界坐标系下的状态转换到IMU的bk帧坐标系下，最初提出预积分的外国大牛市将世界坐标系转换到求状态的变化量，其实两者的原理都一样，预积分求的值都是变化量），两边同时乘以世界到bk帧坐标系的转换，如下图所示，然后提出等式右边只与加速度和角速度有关的量进行积分，如公式5和6：

    到这里其实只要求出公式6中的积分值，真的预积分的值就得到了，这里bk是参考帧，从式6中可以看出，在bk到bk+1帧间，这里要求的三个临时状态量只与IMU的偏置有关系，而与其他状态无关，也就是说每个式子相当于一个二元一次方程（f(x,y)=ax+by+c,x相当于加速度计偏置，y相当于陀螺仪偏置），这里就是为了求解这个二元一次方程，当这里的偏置变化特别小的时候，我们可以使用一阶线性展开来调整临时状态量，如下公式12 所示：

    所以要想求出这个临时的状态量，就必须求出等式右边的两部分值，第一个部分还是原来的积分形式（就像公式6那样），是预积分的主体，论文中使用的是最简单的欧拉积分法进行展开（取第i时刻值的斜率乘以时间差加上i时刻的初值，就得到i+1时刻的值），但是在代码中作者也提出了采用的是中值积分（顾名思义这里的斜率取得是i和i+1中点（2i+1）/2的时刻斜率）.公式7是采用欧拉积分的结果。这里前面有一定的说明，一开始abkbk，bbkbk等是零，旋转是单位旋转，注意整个过程把噪声设为0。

    第二部分其实就是对应的一阶偏导（对加速度计偏置和陀螺仪偏置的），一阶偏导的求法在下面进行介绍，到这里我们已经求出了临时状态量的测量值，也就可以求出状态量测量值。

论文到这一步预积分其实已经做了一半了，也就是完成了测量值的求解，还差什么呢？当然是协方差矩阵了，下面重点求解协方差矩阵，顺便把上面没有求出的陀螺仪和加速度计偏置的雅各比矩阵求出来。

         如何求协方差矩阵呢？怎么从数学的定义里去求呢？这里要用到SLAM中的神作state estimation for robotic，建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。也就是说还是需要前面求解状态测量值的公式6。注意代码中真正求解公式6使用的是中值法，所以为了和代码中相一致，下面的求解过程我也才用中值法的方式，为求解需求我们先补充点干货：

首先需要将上面的四自由度的旋转转换成三个维度的状态量，这是由于四自由度的旋转存在过参数化的情况，因此将误差看成是一个扰动定义式8：

然后有下面的两张图定义出来离散状态下的预积分过程：

    最后得到图中的线性误差状态传播模型，由此得到协方差矩阵和雅各比矩阵，雅各比矩阵可以直接代入到公式12中计算出更加精确的传播状态值，而协方差矩阵自然是在后端优化中使用。到这里恭喜你已经完成了数据前端处理的所有步骤，下面直接进入代码过程吧！