VINS-Mono源码解析（三）后端: IMU预积分

为什么要预积分?
- 论文中的说法是: State Estimation其实是想估计任意时刻的全局坐标系下的位置和姿态, 也就是论文中的$p_{b_k}^w,v_{b_k}^w,q_{b_k}^w$, 分别对应位置,速度,旋转四元数, 它们满足迭代式

$$\begin{equation} \begin{aligned} p_{b_{k+1}}^w &= p_{b_k}^w+v_{b_k}^w\Delta t_k+ \int\int_{t\in[k,k+1]}\left(R_t^w(\hat{a}_t-b_{a_t})-g^w\right)dt^2\\ v_{b_{k+1}}^w &= v_{b_k}^w+ \int_{t\in[k,k+1]}\left(R_t^w(\hat{a}_t-b_{a_t})-g^w\right)dt\\ q_{b_{k+1}}^w &= q_{b_k}^w\bigotimes \int_{t\in[k,k+1]}\frac12\Omega(\hat{\omega}_t-b_{w_t})q_t^{b_k}dt \end{aligned} \end{equation}$$
即当前时刻的状态可以通过上一时刻状态来迭代求得. 但这里有个问题在于积分中有个讨厌的东西 $R_t^w$( $q_t^w$一样), 它是 $t$时刻在$world$系下的旋转, 是一个要估计的量, 后面估计的过程中, 这个量会被不断的更新改变, 如果按照上面的式子, 那么每变一次就得重新积分. 把上次都乘一个旋转 $R_w^{b_k}$转到 $b_k$系下,就可以把那个讨厌的 $R$从积分中抽出来, 这样不管$R_{b_k}^w$怎么变, 我们只需要一些简单的旋转矩阵相乘就能得到世界坐标系下的状态量.
- 个人直观的理解是: 位置,姿态,速度等, 它们的绝对量是跟参考坐标系的选取有关的, 如果参考坐标系移动了, 那么这些量也会跟着改变, 但某两个时刻之间的相对量( $p_{b_{k+1}}^{b_k},v_{b_{k+1}}^{b_k},q_{b_{k+1}}^{b_k}$)是跟坐标选取无关的, 不管在哪个坐标系下看, 相对变化是不变的, 所以可以事先把这些不变的量求出来. $p_{b_k}^w,q_{b_k}^w$这些状态发生改变后, 可以通过矩阵乘法很容易的把相对量转成绝对量.
- 更加简单粗暴的理解: 把相邻两帧图像之间的一段时间的IMU预先积分起来, 得到两帧之间的IMU相对运动, 也就是把多个IMU观测变成一个运动观测, 它是不随某一时刻的绝对位姿改变而发生改变的.

离散形式:

$$\begin{equation} \begin{aligned} \hat{\alpha}_{i+1}^{b_k} &= \hat{\alpha}_{i}^{b_k} + \hat{\beta}_{i}^{b_k} \delta t+\frac12 R(\hat{\gamma}_{i}^{b_k})(\hat{a}_i-b_{a_i})\delta t^2 \\ \hat{\beta}_{i+1}^{b_k} &= \hat{\beta}_{i}^{b_k} +R(\hat{\gamma}_{i}^{b_k})(\hat{a}_i-b_{a_i})\delta t \\ \hat{\gamma}_{i+1}^{b_k}&=\hat{\gamma}_{i}^{b_k}\bigotimes \left[\begin{matrix}1\\ \frac12(\hat{\omega}_i-b_{w_i})\delta t \end{matrix}\right] \end{aligned} \end{equation}$$

对应代码中integration_base.h中的midPointIntegration()函数, 该函数输入两个时刻的IMU观测_acc_0,_acc_1,_gyr_0,_gyr_1, 根据$t_0$时刻的p,q,v,ba,bg, 利用上面公式递推出$t_1$时刻的p,q,v,ba,bg. 每次选完关键帧以后p,q,v,ba,bg又reset为零点.
函数中还有另一块是更新jacobian的, 先求F和V矩阵, 然后根据F和V更新jacobian和covariance. 这里暂时还不是太理解!!!
这段代码对应论文中的公式

$$\delta \dot{z}_t^{b_k} = F\delta z_t^{b_k} + G_tn_t$$
这里, $n_t=[n_a~n_w~n_{b_a}~n_{b_w}]^T$, 是12维, 但代码里面 $n$是18维, 应该是$n_t=[n_{a_0}~n_{w_0}~n_{a_1}~n_{w_1}~n_{b_a}~n_{b_w}]^T$,其中 $n_{a_0}~n_{w_0}~n_{a_1}~n_{w_1}$分别是前一时刻和后一时刻加速度和角速度观测的噪声. $n_{b_a}, n_{b_w}$是bias的噪声,是实不变的.

/* pre-propogation between two successive imu measurements
\param[in] dt: time interval
\param[in] _acc_0: first acc
\param[in] _gyr_0: first gyro
\param[in] _acc_1: second acc
\param[in] _gyr_1: second gyro
\param[in] delta_p: delta position at t0
\param[in] delta_q: delta quaternion at t0
\param[in] delta_v: delta velocity at t0
\param[in] linearized_ba: accelerator bias at t0
\param[in] linearized_bg: gyroscope bias at t0
\param[out] result_delta_p: delta position at t1
\param[out] result_delta_q: delta quaternion at t1
\param[out] result_delta_v: delta velocity at t1
\param[out] result_delta_ba: accelerator bias at t1
\param[out] result_delta_bg: gyroscope bias at t1
\param[in] update_jacobian:  whether to update Jacobian and calculate F,V
*/
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)
{
    //ROS_INFO("midpoint integration");
    Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;              // average angular vel in body frame
    result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); //q1 = q0 * q(0->1)
    Vector3d un_acc_0 =        delta_q * (_acc_0 - linearized_ba);          // un bias acc0 in global frame
    Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);          // un bias acc1 in global frame
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);                          // average un bias acc in global frame
    /*assume constant acceleration from t0 to t1*/
    result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;    // p1 = p0 + v0*t + 0.5*a_avg*t^2
    result_delta_v = delta_v + un_acc * _dt;                                // v1 = v0 + a_avg*t
    /*bias not changed*/
    result_linearized_ba = linearized_ba;
    result_linearized_bg = linearized_bg;         

    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;

        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;

        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;

        //step_jacobian = F;
        //step_V = V;
        jacobian = F * jacobian;
        covariance = F * covariance * F.transpose() + V * noise * V.transpose();
    }
}