VINS-mono代码阅读 -- IMU预积分

最新推荐文章于 2024-04-22 18:52:37 发布

半亩园

最新推荐文章于 2024-04-22 18:52:37 发布

阅读量428

点赞数 3

分类专栏： SLAM 文章标签： slam

本文链接：https://blog.csdn.net/weixin_42580307/article/details/108013876

版权

SLAM 专栏收录该内容

14 篇文章 5 订阅

订阅专栏

这里先给出来几位SLAM前辈推导IMU预积分的文档，由于年代久远找不到大神们的GitHub，我就把文件传到了我的网盘，并在此分享出来。链接：https://pan.baidu.com/s/1DCZoxVRPQJbm_Ls2TXJvfQ，提取码：c0ei。还有邱笑晨博士推的一版，但是我找不到电子版了，如果有人有链接的画，不妨分享在评论区。
找到了崔华坤博士的github:https://github.com/StevenCui/VIO-Doc

1. processIMU()

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
	//1. 判断是不是第一个imu消息，如果是第一个imu消息，就将当前的加速度和角速度作为初始加速度和角速度
    if (!first_imu)//first_imu 标志位，初始值为false
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }
	/* 2. 创建预积分对象
		pre_integrations[]是一个数组，存放了WINDOW_SIZE+1个指针，指针指向的类型是IntegrationBase
		*/
    if (!pre_integrations[frame_count])//**之前没给pre_integrations[frame_count]赋值条件就为真**
    {//当滑窗还没满的时候，或者在移动滑窗的时候，就需要new一个pre_integration
		//acc_0  gyr_0 是前一时刻的加速度和角速度
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
	//frame_count == 0表示窗口中还没有图像帧，所以不进行预积分
    if (frame_count != 0)                                                                                           
    {
		//3. 进行预积分
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)**
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);//临时预积分初始值	//这个push_back是自己写的一个函数

        dt_buf[frame_count].push_back(dt);
		//将当前的线加速度和角速度存放到加速度buf和角速度buf中
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

		/* 4. 更新Rs Ps Vs 三个向量组 */
        int j = frame_count;         
		//计算上一时刻的加速度，前面乘一个 Rs  旋转到第一帧IMU的坐标系
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
		//根据上一个时刻的角速度和当前时刻的角速度求出平均角速度
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
		//计算当前时刻陀螺仪的旋转矩阵，是在上一时刻的旋转矩阵的基础上和当前时刻的旋转增量相乘得到的
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
		//当前时刻的加速度
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
		//位置更新
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
		//速度更新
        Vs[j] += dt * un_acc;
    }
	//积分完成把上一时刻的加速度和角速度赋值为上一时刻的加速度和角速度
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

（1）IMU预积分是通过两个图像帧之间的IMU数据计算出两帧之间的PVQ增量，如果是第一帧的IMU信息，就把当前的值赋值给对应的初始值。
（2）对滑动窗口的每一帧创建预积分对象。每到一个图像帧，就会创建一个IntegrationBase对象存入pre_integrations数组当中。每次更新的acc_0和gyr_0作为IMU上一个状态时候的线加速度和角速度值，Bas[frame_count]和Bgs[frame_count]为对应于id为frame_count这一帧图像的线加速度偏置和角速度偏置值。
（3）预积分是和滑动窗口结合起来的，frame_count表示的是当前窗口中的帧数，如果帧数为0，那么就不需要进行预积分，只需要将加速度和角速度的值进行更新。
（4）帧数不为0，就进行预积分
（5）更新 Rs Ps Vs 三个向量组，滑动窗口和位姿初始化的时候使用。
这里的Rs[j], Ps[j], Vs[j] 是第j帧图像的值。 IMU积分出来的第j时刻的物理量可以作为第j帧图像的初始值。公式也就是一个简单的中值积分。

2. IntegrationBase

在进行预积分之前先创建了一个预积分对象，我们来看一下这个类的构造函数，在创建的时候都对哪些参数进行了初始化。

	//预积分类：加速度，角速度，ba, bg, 雅可比矩阵初始化15*15，协方差矩阵15*15， da, dp, dv, dq
    IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          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();//ACC_N从配置文件中读取
        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();
    }

噪声项的对角协方差矩阵：
在这里插入图片描述
预积分类中的成员变量

    double dt;//每次预积分的时间长度
    Eigen::Vector3d acc_0, gyr_0;//t时刻对应的IMU测量值
    Eigen::Vector3d acc_1, gyr_1;//t+1时刻对应的IMU测量值0

    const Eigen::Vector3d linearized_acc, linearized_gyr;//k帧图像时对应的IMU测量值
    Eigen::Vector3d linearized_ba, linearized_bg;//加速度及的ba，bg
		
    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;//噪声的对角协方差矩阵

    double sum_dt;//所有IMU预积分区间的总时长
    Eigen::Vector3d delta_p;//位置
    Eigen::Quaterniond delta_q;//旋转四元数
    Eigen::Vector3d delta_v;//速度

    std::vector<double> dt_buf;//存储每次预积分时间长度dt的buf
    std::vector<Eigen::Vector3d> acc_buf;//每次预积分加速度量
    std::vector<Eigen::Vector3d> gyr_buf;//角速度

3. 预积分

push_back() 将变量放进对应的buf中，调用propagate() 进行预积分

    void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        propagate(dt, acc, gyr);
    }

	//计算两个关键帧之间IMU测量的变化量
    void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
    {
        dt = _dt;
        acc_1 = _acc_1;
        gyr_1 = _gyr_1;
        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,  ,
                            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);
		//更新 P Q V
        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;
		//预积分完成后，更新当前的线加速度和角速度为上一时刻的线加速度和角速度
        acc_0 = acc_1;
        gyr_0 = gyr_1;  
     
    }

propagate() 中主要调用的是midPointIntegration()中值积分完成的预积分计算。

	/*
		_0代表上次的测量，_1代表当前的测量值
		delta_p, delta_q, delta_v 代表相对预积分初始参考系的位移，旋转四元数以及速度
		从k帧积分到k+1帧，则参考坐标系是k帧的imu坐标
	*/
    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");
		//delta_q 为相对预积分参考系的旋转四元数，线加速度的测量值减去偏置，然后和四元数相乘表示将线加速度   从世界坐标系下  转换到   参考坐标系下
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
		//平均角速度
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
		//角速度和时间的乘积 转换成   左乘旋转   四元数， 获得当前时刻body中的旋转向量（四元数表示）
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
		//用计算出来的旋转向量左乘当前时刻加速度，将线加速度从世界坐标系下转到了body系下
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
		//预积分的过程中bias未发生变化
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         

        if(update_jacobian)
        {
			//平均角速度
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
			//计算 _acc_0 这个观测线加速度的实际值
            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;

			/** 
			*         |0	-W_z	W_y  |
			* [W]_x = |W_z	  0		-W_x |
			*         |-W_y	 W_x	  0	 |
			*/
            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);
			/**
			* 离散形式的PVQ增量误差分析
			* matrix.block(i,j,p,q): 从 (i,j)开始，取一个p*q大小的矩阵，原矩阵不变
			* matrix.block<p,q>(i,j): p行q列的子矩阵，从原矩阵中第(i,j)个元素开始，获取一个p*q的子矩阵，原矩阵不变
			*/
            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;
			//雅可比矩阵的迭代公式：J_(k+1) = F * J_(k)   J_(0) = I ;
            jacobian = F * jacobian;
			//covariance 是协方差，协方差的迭代公式：P_(k+1) = F * P_(k) * F^T + V * Q * V^T , P_(0) = 0;
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();
        }

    }

midPointIntegration()中有一个操作就是更新雅可比矩阵，这个雅可比矩阵的更新是什么样的操作呢？

IMU在每一时刻积分出来的值都是有误差的，直接给出在t时刻误差项的倒数为：
在这里插入图片描述
其中： $F_t^{15\times15},G_t^{15\times12},\delta z_t^{b_k^{15\times1}},n_t^{12\times1}$
将上式简写为： $\delta \dot{z}_t^{b_k}=F_t \delta z_t^{b_k}+G_tn_t \tag 1$

根据导数的定义有：
$\delta z_{t+\delta t}^{b_k}=\delta z_t^{b_k}+ \delta \dot{z}_t^{b_k}\delta t \tag 2$
联合上面的两个式子：
$\delta z_{t+\delta t}^{b_k}=(I+F_t\delta t)\delta z_t^{b_k}+(G_t\delta t)n_t \tag 3$
令 $(I+F_t\delta t) = F,G_t\delta t=V$
得到：
$\delta z_{t+\delta t}^{b_k}=F\delta z_t^{b_k}+Vn_t \tag 4$
4式就是IMU误差运动方程，将上式和EKF对比，上式恰好给出了和EKF一样对非线性系统线性化的过程，这里给出的意义是表示下一时刻的IMU测量误差与上一时刻的成线性关系，这样就可以根据当前时刻的值，预测下一时刻的均值和协方差。上面给出了均值的预测，那么协方差的预测公式：
$P_{t+\delta t}^{b_k}=(I+F_t\delta t)P_t^{b_k}(I+F_t\delta t)^T+(G_t\delta t)Q(G_t\delta t)^T \tag 5$
初始值 $p_{b_k}^{b_k}=0$
根据4可以获得误差雅可比的迭代公式：
$J_{t+\delta t}=(I+F_t\delta t)J_t$
其中雅可比的初始值为： $J_{b_k}=I$
上面的这些公式是连续形式下的PVQ增量的误差，协方差和雅可比，在这里仅仅是为了说明误差雅可比的更新方式，关于离散形式下的PVQ增量的误差，协方差和雅可比，请见参考文献。

至此，IMU预积分部分就结束了。

半亩园

关注

3
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
VINS-mono代码阅读 -- IMU预积分

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity){ //1. 判断是不是第一个imu消息，如果是第一个imu消息，就将当前的加速度和角速度作为初始加速度和角速度 if (!first_imu)//first_imu 标志位，初始值为false { first_imu = true;
复制链接

扫一扫

专栏目录