VINS学习（二）IMU预积分原理与实现

一、连续时间下的IMU积分

IMU测量值是在IMU坐标系中测量的，它受到加速度偏置$a_t$、陀螺仪偏置$b_t$和噪声$n_a$的影响。假设加速度计和陀螺仪测量值中的噪声为高斯噪声:

IMU坐标系对应$b_k,b_{k+1}$，对于图像帧$k$和$k+1$，在$[t_k,t_{k+1}]$时间间隔内对所有IMU测量值进行积分，可得第 $k+1$ 帧的位置、速度和旋转 （$PVQ$）在世界坐标系下进行传递:

其中：

关于四元数$q=[q_w,q_x,q_y,q_z]$求导的补充：
$$\begin{array}{rl}{\dot{q}}_t^w& =\underset{\Delta t\to 0}{\lim }\frac{q_{t+\Delta t}^w-q_t^w}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{q_t^w\otimes q_{t+\Delta t}^t-q_t^w\otimes I_{(q)}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{q_t^w\otimes \left[\begin{array}{c}cos\frac{\theta }{2}\\ \vec{n}\frac{sin\theta }{2}\end{array}\right]-q_t^w\otimes \left[\begin{array}{c}1\\ \vec{0}\end{array}\right]}{\Delta t}\\ & \approx \underset{\Delta t\to 0}{\lim }\frac{q_t^w\otimes \left[\begin{array}{c}1\\ \frac{\vec{n}\theta }{2}\end{array}\right]-q_t^w\otimes \left[\begin{array}{c}1\\ \vec{0}\end{array}\right]}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{{\left[\begin{array}{c}{q}_{t+\Delta t}^t\end{array}\right]}_R{q}_t^w-{\left[\begin{array}{c}{I}_{(q)}\end{array}\right]}_R{q}_t^w}{}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\Omega (\frac{\vec{n}\theta }{2})q_t^w}{\Delta t}\\ & =\frac{1}{2}\Omega (\hat{w})q_t^w\end{array}$$
关于四元数乘法的补充：

二、连续时间下的IMU预积分

在卡尔曼滤波中，假设了一阶马尔可夫性，当前时刻状态值只与上一时刻的状态值有关，所以历史时刻的状态不会发生改变，使用普通的方法对IMU进行积分，单向传播即可。但是，在当我们后端进行非线性优化时，历史时刻的状态变量PVQ以及IMU的bais会随着每次迭代而改变，由于IMU积分与上一时刻的状态量相关，所以每次调整完之后，都需要重新计算IMU积分， 造成重复转播，为了解决这个问题引入了IMU预积分方法：
将参考坐标系从世界坐标系（$w$）转变为当前帧坐标系（$b_k$系):

其中：

这样我们就得到了连续时刻的 IMU 预积分公式，可以发现，上式得到的 IMU 预积分的值只与不同时刻的${\hat{a}}_t$和${\hat{w}}_t$相关(实际上预积分值与我们优化变量IMU的bias 也是相关的，但是我们放在后面讨论，连续情况下的预积分先到此为止)。显然，当上一时刻的状态量变化时，预积分值不发生改变，只需要重新简单的计算加减法即可。

三、离散时间下的IMU预积分

1. 欧拉法

下面给出离散时刻的 IMU 预积分公式，首先按照论文中采用的欧拉法，给出第 $i$ 个 IMU时刻与第 $i+1$ 个 IMU 时刻的变量关系为：

2. 中值法

下面给出代码中采用的基于中值法的 IMU 预积分公式，这里积分出来的是前后两帧之间的 IMU 增量信息，而不是当前帧时刻的物理量信息:
$$\begin{array}{rl}{\hat{\alpha }}_{i+1}^{b_k}& ={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}{\bar{\hat{\alpha }}}_i\delta t^2\\ {\hat{\beta }}_{i+1}^{b_k}& ={\hat{\beta }}_i^{b_k}+{\bar{\hat{\alpha }}}_i\delta t\\ {\hat{\gamma }}_{i+1}^{b_k}& ={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\overline{{\hat{\omega }}_i\delta t}\end{array}\right]\end{array}$$
其中：
$$\begin{array}{rl}\overline{{\hat{\alpha }}_i}& =\frac{1}{2}[q_i^{b_k}({\hat{\alpha }}_i-b_{a_i})+q_{i+1}^{b_k}({\hat{\alpha }}_{i+1}^{b_k}-b_{{\alpha }_i})]\\ \overline{{\hat{\omega }}_i}& =\frac{1}{2}({\hat{\omega }}_i+{\hat{\omega }}_{i+1})-b_{{\omega }_i}\end{array}$$

四、连续时间下的IMU状态误差传递

IMU 在每一个时刻积分出来的值是有误差的，下面我们对误差进行分析。首先我们直接给出在 t 时刻误差项的导数为（更具体的推导可以参考我之前的一篇博客IMU预积分模型分析）：

可以简写为：

根据导数定义可知，下一时刻的均值预测为：

协方差预测公式如下：

在协方差的迭代公式中初始值$P_{b_k}^{b_k}=0$，$Q$表示噪声的协方差矩阵：

误差项的 Jacobian 初始值为$J_{b_k}=I$，迭代公式如下：

五、离散时间下的IMU状态误差传递

考虑离散时间下的IMU状态误差传递：
$$\begin{array}{rl}\delta z_{k+1}& =\delta z_k+J(x)\Delta t\end{array}$$
利用连续时间下的推导，最终可以得到增量误差在离散形式下的矩阵形式：

最终离散时间下矩阵形式可以表达为：

$Q$ 为表示噪声项的对角协方差矩阵:

离散时间下预积分协方差矩阵的传递可以表示为（$P_k$初值为0）：

六、预积分量关于零偏的雅克比

在第二节中提到预积分值与我们优化变量IMU的bias 也是相关的，而bias 也是我们需要优化的变量，如果每次优化后因为bias改变需要重新计算预积分的话，那么预积分的引入将毫无意义。所以我们这里假设预积分的变化量与bias 是线性关系，则可以表示为：

当优化后bais发生改变时，我们使用上式近似校正预积分结果，而不重新计算预积分。
显然这样计算会带来一个新的问题：$J_{b_a}^{\alpha }、J_{b_w}^{\alpha }、J_{b_a}^{\beta }、J_{b_w}^{\beta }、J_{b_w}^{\gamma }$怎么计算。想要计算出这几个雅可比矩阵的闭式解是困难的，我们考虑估计值对本身的求导：$J_{z_{k+1}}=\frac{\partial z_{k+1}}{\partial z_k}$，显然有：
$$\begin{array}{rl}{J}_{b_a}^{\alpha }& =J_{z_{k+1}}(0,1)\\ J_{b_w}^{\alpha }& =J_{z_{k+1}}(0,4)\\ J_{b_a}^{\beta }& =J_{z_{k+1}}(1,0)\\ J_{b_w}^{\beta }& =J_{z_{k+1}}(1,4)\\ J_{b_w}^{\gamma }& =J_{z_{k+1}}(2,4)\end{array}$$
根据链式求导法，$J_{z_{k+1}}$可由$J_{z_k}$（初值为单位阵$I$）递推得到：
$$J_{z_{k+1}}=.........F_{i=1}^{i=2}{F}_{i=0}^{i=1}{J}_{z_k}$$

七、VINS代码实践

VINS中关于IMU预积分的代码都集中在integration_base.h中，实现了一个IntegrationBase类。

1.预积分类的数据成员与构造函数

    double dt; //IMU帧的时间间隔
    Eigen::Vector3d acc_0, gyr_0; //当前帧传入的IMU加速度和角速度
    Eigen::Vector3d acc_1, gyr_1;//上一帧IMU加速度和角速度

    const Eigen::Vector3d linearized_acc, linearized_gyr;//当前帧传入的IMU加速度和角速度
    Eigen::Vector3d linearized_ba, linearized_bg;//上一帧IMU加速度和角速度的bais

    Eigen::Matrix<double, 15, 15> jacobian, covariance;//雅可比矩阵和协方差矩阵
    Eigen::Matrix<double, 15, 15> step_jacobian;
    Eigen::Matrix<double, 15, 18> step_V;
    Eigen::Matrix<double, 18, 18> noise;//噪声矩阵 包括 n_ak n_wk n_ak+1 n_wk+1 n_ba n_bw

    double sum_dt;  //关键帧的时间间隔
    Eigen::Vector3d delta_p; //位移增量
    Eigen::Quaterniond delta_q;//旋转增量
    Eigen::Vector3d delta_v;//速度增量

    std::vector<double> dt_buf;//关键帧之间的IMU帧时间间隔
    std::vector<Eigen::Vector3d> acc_buf;//关键帧之间的IMU帧加速度
    std::vector<Eigen::Vector3d> gyr_buf;//关键帧之间的IMU帧角速度

IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 
                //预积分开始时（初始时刻）的IMU加速度和角速度
                
               const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) 
               //一次预积分的IMU加速度和角速度的bais（不会改变）
               
   : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
   //利用传入的IMU加速度和角速度给初始时刻和上一时刻的数据赋值
   
     linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg}, 
     //预积分的IMU加速度和角速度的bais给数据成员赋值
     
       jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
       //预积分雅可比矩阵初值为单位阵，协方差矩阵为0
       
     sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
     //位移、速度、旋转的增量初始化

{
   noise = Eigen::Matrix<double, 18, 18>::Zero();
   //初始化噪声矩阵
   noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
   noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
   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();
   noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
}

2.添加IMU数据

在类的构造函数初始化预积分数据之后，我们基于当前图像关键帧进行IMU预积分操作，所以需要多次添加IMU帧数据，添加函数为push_back。得到每一帧IMU数据之后进行保存并进行传播。

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
    // 相关时间差和传感器数据保留，方便后续repropagate
    dt_buf.push_back(dt); //IMU帧之间的时间间隔
    acc_buf.push_back(acc);//当前IMU帧的加速度
    gyr_buf.push_back(gyr);//当前IMU帧的角速度
    propagate(dt, acc, gyr);//传播函数 计算预积分并更新协方差矩阵
}

3.根据IMU数据进行预积分

dt = _dt;//IMU帧间隔
acc_1 = _acc_1;//当前帧IMU加速度
gyr_1 = _gyr_1;//当前帧IMU角速度


Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
//定义变量存储预积分结果


midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                    linearized_ba, linearized_bg,
                    result_delta_p, result_delta_q, result_delta_v,
                    result_linearized_ba, result_linearized_bg, 1);

//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
//                    linearized_ba, linearized_bg);
//将中值预积分得到的结果进行赋值
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
//四元数结果需要归一化
delta_q.normalize();
sum_dt += dt;
//将当前帧IMU数据保存为上一帧
acc_0 = acc_1;
gyr_0 = gyr_1;  

预积分中最主要的函数是midPointIntegration，实现了一次IMU中值预积分：
参数说明：

void midPointIntegration(
double _dt, 
//两帧IMU的时间间隔

      const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
      //上一帧的IMU数据
      
      const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
      //当前帧的IMU数据
      
      const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
      //上一IMU帧的位移 速度 旋转的增量  (已知)
      
      const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
      //预积分过程中的IMU bais  (已知，一次预积分过程中不变)
      
      Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
      ///当前IMU帧的位移 速度 旋转的增量  （待求）
      
      Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, 
      预积分过程中的IMU bais  (待求 ，直接由linearized_ba、linearized_bg赋值)
      
      bool update_jacobian)
      //是否更新雅可比矩阵

代码细节：

  Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
  //根据上一IMU帧的四元数 将上一帧IMU加速度去bais 变换到b_k坐标系下

  Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
  //根据上一帧和当前帧IMU角速度计算中值角速度

  result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
  //计算当前帧四元数 旋转角度比较小 \gamma_{k+1} 近似为 [1,\theta /2]

  Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
  //根据当前IMU帧的四元数 将当前IMU帧加速度去bais 变换到b_k坐标系下

  Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
  //根据上一帧和当前帧IMU加速度（b_k系下）计算中值加速度

  result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
  //计算位移增量

  result_delta_v = delta_v + un_acc * _dt;
  //计算速度增量

  result_linearized_ba = linearized_ba;
  result_linearized_bg = linearized_bg;      
  //两图像关键帧之间的预积分认为bais不变 所以 直接赋值

接下来的代码主要是根据第五节中的离散化公式计算$F$矩阵以及$V$矩阵。为了简化计算过程，作者提前先计算了三个反对称矩阵：$({\hat{a}}_k-b_{a_k}{)}^{\wedge }$ 、$({\hat{a}}_{k+1}-b_{a_k}{)}^{\wedge }$ 、$(\frac{{\hat{w}}_k+{\hat{w}}_{k+1}}{2}-b_{w_k}{)}^{\wedge }$

  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;

接下来套用公式计算（具体实现见源代码及公式）：

 MatrixXd F = MatrixXd::Zero(15, 15);
 //略具体赋值 
 
 MatrixXd V = MatrixXd::Zero(15,18);
 //略具体赋值 

值得注意的是，在在计算$V$矩阵时，关于$n_{a_k}、n_{a_{k+1}}、n_{w_{k+1}}、n_{w_k}$的系数与前面的计算相差一个符号，但是由于是均值为0的高斯白噪声，所以其效果是一样的。

最后每次预积分更新雅可比$J$和协方差$P$：

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