VINS中IMU预积分推导

20 篇文章 20 订阅
4 篇文章 2 订阅

VIO 中,如果在世界坐标系中对 IMU 进行积分,积分项中包含体坐标系相对于世界坐标系的瞬时旋转矩阵。然而,在优化位姿时,关键帧时刻体坐标系相对于世界坐标系的旋转矩阵会发生变化,那么需要对 IMU 重新进行积分。预积分就是为了避免这种重复积分。IMU 预积分将参考坐标系改为前一帧的体坐标系,从而积出了两帧之间的相对运动。

预积分

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

de4af11c893081a0df0cf84c8e2417f9.png

8c2b3f7b131d58f218f1ff30b0415ad0.png

(点击图片放大看公式)

连续形式

f5684974e10960e7a461e2ad173ce618.png

336f0ddd153255324bc71475c649d642.png

离散形式

3ee2618b1557e516b4431348ef2dfaac.png

//采用的是中值积分的传播方式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;
 
 

(左右滑动试试)

2edce04ced6b8bee41ca3ee7d2b722c1.png

bf3ec2f170a4e3df1cd471d24496c3c6.png

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_* _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();
 
 

(左右滑动试试)

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 和协方差

f5fd34a9f2461202828b91aae90e3ca5.png

对应代码在integration_base.h文件的midPointIntegration():

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

84fb3314b1d057e7ab2838137c1ab54e.png

  • 1
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值