[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——IMU预积分(内容|代码)

理论知识之前在另一篇博客也整理过 ——指路

这篇博客梳理的是integration_bash.h中的midPointIntegration()中的一系列操作。

1. 为什么要进行IMU积分？

IMU通过加速度计得到线加速度、通过陀螺仪得到角速度。通过对IMU测量值的积分操作，能够获得机器人的位姿信息。

IMU的频率比相机的要高，将第 k 帧和第 k+1 帧之间的所有 IMU 进行积分,可得第 k+1 帧的位置、速度和旋转(PVQ)，作为视觉估计的初始值，示意图如下：

从IMU获取的IMU坐标系下的测量值：

2. 连续时间下的IMU运动模型

对于两个连续的关键帧$b_k$bk​和$b_{k+1}$bk+1​，其对应的时刻分别是$t_k$tk​、$t_{k+1}$tk+1​，从第$t_k$tk​时刻的PVQ对IMU的测量值进行积分，可以得到第$t_{k+1}$tk+1​时刻的PVQ,公式为：

3.为什么要用IMU预积分？

注意，此时的积分中包含IMU坐标系到世界坐标系的瞬时旋转矩阵$q_{b_t}^w$qbt​w​，然而,在优化位姿时,关键帧时刻的IMU坐标系相对于世界坐标系的旋转矩阵会发生变化,那么需要对 IMU 重新进行积分。

因此采用预积分来避免这种重复积分。IMU 预积分将参考坐标系从世界坐标系改为前一帧的IMU坐标系$b_k$bk​,从而积出了两帧之间的相对运动。

代入IMU积分公式得：

IMU的预积分量

整理一下得到IMU的预积分量：

可以看到，预积分量只跟IMU的测量值和IMU的bias有关，我们假设短时间内IMU的bias是保持不变的，重新整理PVQ的公式可得：

IMU的预积分误差

一段时间内IMU构建的预积分量作为测量值，对两个时刻之间的状态量进行约束，得到IMU的预积分误差：
以上的误差中位移，速度，偏置都是直接相减得到的。第二项是关于四元数的旋转误差，

预积分的离散形式

离散形式有两种方法：中值法和欧拉法。这里只分析中值法：
也就是两个相邻时刻$k$k到$k+1$k+1的位姿是用两个时刻的测量值$a$a,$w$w的平均值来计算：


代码：

 void midPointIntegration(double _dt,                             
                          const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,                            
                          const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,                            
                          const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,                            
                          const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,                            
                          Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,                            
                          Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)    
{

4. 误差传播

将测量噪声考虑进来，更改PVQ模型：

预积分误差传播的形式

相关代码在integration_bash.h中的midPointIntegration()：

if(update_jacobian)        
{            
    Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;            
    Vector3d a_0_x = _acc_0 - linearized_ba;            
    Vector3d a_1_x = _acc_1 - linearized_ba;            
    Matrix3d R_w_x, R_a_0_x, R_a_1_x;
            
    R_w_x<<0, -w_x(2), w_x(1),                
           w_x(2), 0, -w_x(0),                
           -w_x(1), w_x(0), 0;            
    R_a_0_x<<0, -a_0_x(2), a_0_x(1),                
             a_0_x(2), 0, -a_0_x(0),                
             -a_0_x(1), a_0_x(0), 0;            
    R_a_1_x<<0, -a_1_x(2), a_1_x(1),                
             a_1_x(2), 0, -a_1_x(0),                
             -a_1_x(1), a_1_x(0), 0;

F矩阵：

MatrixXd F = MatrixXd::Zero(15, 15);            
F.block<3, 3>(0, 0) = Matrix3d::Identity();            
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +                                   
                      -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;            
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;            
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;            
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;            
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;            
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;            
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +                                   -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;            
F.block<3, 3>(6, 6) = Matrix3d::Identity();            
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;            
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;            
F.block<3, 3>(9, 9) = Matrix3d::Identity();            
F.block<3, 3>(12, 12) = Matrix3d::Identity();            //cout<<"A"<<endl<<A<<endl;

V矩阵

MatrixXd V = MatrixXd::Zero(15,18);            
V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;            
V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;            
V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;            
V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);            
V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;            
V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;            
V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;            
V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);            
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;            
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

离散形式的 PVQ 增量误差的 Jacobian 和协方差

预积分误差传播的形式可以简写为：


对应代码：

Q矩阵——噪声项的对角协方差矩阵：

noise = Eigen::Matrix<double, 18, 18>::Zero();      
noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); // 加速度的噪声：n_a        
noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); // 陀螺仪的噪声：n_g        
noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();        
noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();        
noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); // 加速度偏置的噪声n_ba        
noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); // 陀螺仪偏置的噪声n_bg

雅可比J和协方差矩阵的迭代：

jacobian = F * jacobian;            
covariance = F * covariance * F.transpose() + V * noise * V.transpose();