彻底弄懂IMU预积分原理及代码实现

于 2024-05-10 21:06:21 发布
阅读量1.1k 收藏 16
点赞数 13
文章标签： 人工智能 自动驾驶 计算机视觉
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/adas_l5/article/details/138681856
版权

彻底弄懂IMU预积分原理及代码实现

image

附赠自动驾驶学习资料和量产经验：链接

1. IMU测量模型

image

IMU测量模型

2.相邻两个IMU数据之间的积分递推，时间间隔 image 恒定

image

相邻两个imu数据之间的递推

image

IMU 预积分示意图

3. 从i时刻积分到j时刻的递推公式，中间包含很多个IMU数据，每个IMU数据之间的时间间隔都是 image

image

动力学积分递推公式

4. 写成预积分形式，使得预积分量不和i时刻和j时刻的状态量相关，这样每次更新i时刻和j时刻的状态量时就不需要重新计算预积分量

image

预积分原始公式

image

于是将预积分项 Δ� 写成递推形式有：

image

image

image

5. 真实值和估计值之间，由于白噪声的存在，会有一定的误差，假设白噪声服从高斯分布，那么状态向量的估计值的协方差矩阵可以递推，即不确定度的递推，先分离出误差形式。

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

image

image

6. 白噪声的高斯分布证明及协方差递推

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

image

状态量的协方差递推

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

7. 当状态向量的bias发生更新的时候，预积分项需要重新进行积分，通常bias的增量都非常小，直接采用一阶近似来进行更新，而不用重新计算整个预积分过程，此时Jacobian可以利用递推进行计算。

image

bias的一阶线性展开

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

image

image

image

image

预积分量对bias的jacobian

8. 完整积分过程

image

增量式预积分迭代计算

9. 在和视觉或者其他传感器融合的时候，我们可以利用IMU的估计值，和其他传感器的真实值的差值来作为残差项，从而迭代的求解出最优解，此时估计量为以下形式，需要计算出每一个残差项对待求解变量的jacobian。

image

和其他传感器融合时的残差项

定义每个变量的增量

image

“lift-solve-retract” method

image

image

image

image

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

image

外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传

if (jacobians) {
            if (jacobians[0]) {
                Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> dr_dq_i(jacobians[0]);
                dr_dq_i.setZero();
                dr_dq_i.block<3, 3>(ES_Q, 0) = -right_jacobian(r.segment<3>(ES_Q)).inverse() * q_j.conjugate().matrix() * q_center_i.matrix();
                dr_dq_i.block<3, 3>(ES_P, 0) = imu_i.q_cs.conjugate().matrix() * hat(q_center_i.conjugate() * (p_j - p_center_i - dt * v_i - 0.5 * dt * dt * gravity));
                dr_dq_i.block<3, 3>(ES_V, 0) = imu_i.q_cs.conjugate().matrix() * hat(q_center_i.conjugate() * (v_j - v_i - dt * gravity));
                dr_dq_i = pre.delta.sqrt_inv_cov * dr_dq_i;
            }
            if (jacobians[1]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dp_i(jacobians[1]);
                dr_dp_i.setZero();
                dr_dp_i.block<3, 3>(ES_P, 0) = -q_i.conjugate().matrix();
                dr_dp_i = pre.delta.sqrt_inv_cov * dr_dp_i;
            }
            if (jacobians[2]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dv_i(jacobians[2]);
                dr_dv_i.setZero();
                dr_dv_i.block<3, 3>(ES_P, 0) = -dt * q_i.conjugate().matrix();
                dr_dv_i.block<3, 3>(ES_V, 0) = -q_i.conjugate().matrix();
                dr_dv_i = pre.delta.sqrt_inv_cov * dr_dv_i;
            }
            if (jacobians[3]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dbg_i(jacobians[3]);
                dr_dbg_i.setZero();
                dr_dbg_i.block<3, 3>(ES_Q, 0) = -right_jacobian(r.segment<3>(ES_Q)).inverse() * expmap(r.segment<3>(ES_Q)).conjugate().matrix() * right_jacobian(dq_dbg * dbg) * dq_dbg;
                dr_dbg_i.block<3, 3>(ES_P, 0) = -dp_dbg;
                dr_dbg_i.block<3, 3>(ES_V, 0) = -dv_dbg;
                dr_dbg_i.block<3, 3>(ES_BG, 0) = -Eigen::Matrix3d::Identity();
                dr_dbg_i = pre.delta.sqrt_inv_cov * dr_dbg_i;
            }
            if (jacobians[4]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dba_i(jacobians[4]);
                dr_dba_i.setZero();
                dr_dba_i.block<3, 3>(ES_P, 0) = -dp_dba;
                dr_dba_i.block<3, 3>(ES_V, 0) = -dv_dba;
                dr_dba_i.block<3, 3>(ES_BA, 0) = -Eigen::Matrix3d::Identity();
                dr_dba_i = pre.delta.sqrt_inv_cov * dr_dba_i;
            }
            if (jacobians[5]) {
                Eigen::Map<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> dr_dq_j(jacobians[5]);
                dr_dq_j.setZero();
                dr_dq_j.block<3, 3>(ES_Q, 0) = right_jacobian(r.segment<3>(ES_Q)).inverse() * imu_j.q_cs.conjugate().matrix();
                dr_dq_j.block<3, 3>(ES_P, 0) = -q_i.conjugate().matrix() * q_center_j.matrix() * hat(imu_j.p_cs);
                dr_dq_j = pre.delta.sqrt_inv_cov * dr_dq_j;
            }
            if (jacobians[6]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dp_j(jacobians[6]);
                dr_dp_j.setZero();
                dr_dp_j.block<3, 3>(ES_P, 0) = q_i.conjugate().matrix();
                dr_dp_j = pre.delta.sqrt_inv_cov * dr_dp_j;
            }
            if (jacobians[7]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dv_j(jacobians[7]);
                dr_dv_j.setZero();
                dr_dv_j.block<3, 3>(ES_V, 0) = q_i.conjugate().matrix();
                dr_dv_j = pre.delta.sqrt_inv_cov * dr_dv_j;
            }
            if (jacobians[8]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dbg_j(jacobians[8]);
                dr_dbg_j.setZero();
                dr_dbg_j.block<3, 3>(ES_BG, 0).setIdentity();
                dr_dbg_j = pre.delta.sqrt_inv_cov * dr_dbg_j;
            }
            if (jacobians[9]) {
                Eigen::Map<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> dr_dba_j(jacobians[9]);
                dr_dba_j.setZero();
                dr_dba_j.block<3, 3>(ES_BA, 0).setIdentity();
                dr_dba_j = pre.delta.sqrt_inv_cov * dr_dba_j;
            }
        }

参考文献

  1. Forster C, Carlone L, Dellaert F, et al. On-Manifold Preintegration for Real-Time Visual–Inertial Odometry[J]. IEEE Transactions on Robotics, 2016, 33(1): 1-21.

  2. Li J, Yang B, Huang K, et al. Robust and Efficient Visual-Inertial Odometry with Multi-plane Priors[C]//Chinese Conference on Pattern Recognition and Computer Vision (PRCV). Springer, Cham, 2019: 283-295.

  3. itsuhane/slamtools

  4. https://github.com/zju3dv/PVIO

adas_l5
关注
  • 13
    点赞
  • 踩
  • 16
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
IMU预积分
weixin_38216573的博客
06-18 3320
为什么要预积分？ 首先是因为IMU和图像频率不一致的问题，相邻两个图像帧之间的IMU信息需要积分从而与视觉信息对齐。 其次，当直接对上一时刻bk进行积分时得到的是bk+1时刻下，IMU坐标原点在世界坐标系上的坐标、速度和旋转角度。当Rw<-t,也就是qw<-t，是需要被优化的量，而这个优化的量，是在IMU积分符号内部的，在后续的优化过程中，这个值是在不断的变化，所以需要一遍遍的重新IMU积分才能获得真正精确的PVQ值。为了减少IMU积分次数，就希望这个被优化的量不要出现在积分符号里，所以就采用I
VIO+VINS-mono中的IMU预积分和代码实现.pdf
最新发布
05-09
[VIO]+VINS-mono中的IMU预积分和代码实现.pdf
IMU预积分总结与公式推导.pdf
11-11
泡泡机器人北航博士邱笑晨IMU积分公式总结和推导，很全面也很详细的知道，学习learnVIorb 代码以及VINS必备的基础知识总结， 公式推导的很详细，涉及到代码中的很多指导过程
imu预积分总结和推导.pdf
10-27
对Foster 的paper中的公式进行了详尽的推导，试图将这套优雅的理论详细地展现在读者面前，使读者对IMU 预积分理论有更加完备的认识
imu预积分
qq_40464599的博客
12-31 723
我觉得这篇文章写得很好，解决了我之前对预积分的问题，文中的粗体加黑就是预积分的意义！ 原文参考：https://lgg2018.com/2019/12/15/imu%E9%A2%84%E7%A7%AF%E5%88%86/ 在VIO的入门必读类中，有一篇是讲IMU预积分的，Forster的文章写的很精炼，还有部分内容在补充材料里。这里强推丘大神的中文版解析，其在文献基础上对背景和公式都做了详细解释和清晰的推导，可能在零星几个小地方稍有瑕疵，但绝对值得一读，对初入门者梳理相关公式的推导有莫大助力！ 可在之
IMU状态预积分测量模型
月照银海似蛟龙的博客
03-25 620
通过上面的推导，定义了预积分的三个观测量和它们的噪声。将它们代回最初的定义式，可以简单写为从公式中可以看到预积分的几大优点。它的左侧是可以通过传感器数据积分得到的观测量，右侧是根据状态变量推断出来的预测值，再加上（或乘上）一个随机噪声。左侧变量的定义方式非常适合程序实现。一方面，△Rik△Rik​可以通过IMU的读数得到，△vik△vik​可以由k时刻IMU的读数加上△Rik△Rik​算得，而△pik△p​ik​。
IMU导航解算程序
06-27
导航解算 从IMU的加表和陀螺数据解算飞行器的位置速度
VINS 中的IMU预积分推导和代码解读.zip
06-06
VINS 中的IMU预积分推导和代码解读，内含自动驾驶完整学习资料
ORB3 IMU预积分公式参考
05-19
ORB3 IMU预积分公式参考主要涉及的是在视觉惯性导航（Visual-Inertial Navigation System, VINS）中，如何利用IMU（惯性测量单元）的数据进行预积分处理，以提高定位和姿态估计的精度。预积分技术最初由T Lupton提出...
关于VIO中IMU预积分的讲解
guoyunlei的博客
05-28 1万+
Why VIO 转自：https://zhehangt.github.io/2019/03/23/SLAM/Basic/VIOInit/ 首先我们先简单回顾一下为什么要做VIO，以及为什么要做VIO初始化。 我们知道单目相机在做SLAM的时候是缺乏实际的尺度信息的，而且尺度还会产生漂移。如果缺乏实际的尺度信息，此时的定位信息将很难进行实际的应用。在面对一些极端环境的时候或者突然运动过快的时候，...
IMU预积分【SLAM】
Tong_1024_的博客
03-29 784
预积分的推导过程比较多，所以这里只记录关键结论。 其实这些公式不太好记忆，因为预积分推导过程的想法来源很巧妙，无法看出物理意义。一切推导的来源： 最好记忆的旋转相对运动量： △Rij≐RiTRj=∏k=ij−1Exp((ω~k−bg,k−ηgd,k)△t)\triangle R_{ij} \doteq R_{i}^{T}R_{j} = \prod_{k=i}^{j-1}Exp((\tilde{\omega}_{k}-b_{g,k}-\eta_{gd,k})\triangle t) △Rij​≐RiT​Rj​
[SLAM] IMU预积分推导
qq_41265365的博客
08-23 1184
本文内容主要来自于对邱笑晨博士IMU.pdf和高博对预积分讲解视频的个人理解。
imu预积分_特大喜讯！IMU预积分超详细推导修订版出炉！
weixin_39623244的博客
12-01 128
号外！号外！错误百出的《IMU预积分总结与公式推导》终于出修订版了！修正了关于残差Jacobian的描述问题（详情参见本专栏上一篇文章），修正了部分下标。欢迎移步泡泡机器人论坛下载。2020.05.30 edit：修改标题便于搜索。...
IMU预积分--详细推导过程
热门推荐
weixin_51547017的博客
12-27 1万+
IMU预积分推导
科氏加速度修正IMU预积分c++eigen代码
09-23
科氏加速度修正（Covariance Stabilizing Correction）是在惯性测量单元（IMU）的预积分中使用的一种技术，用于修正由于误差累积导致的姿态估计误差。Covariance Stabilizing Correction方法的实现通常需要使用Eigen库进行矩阵运算。 以下是一个使用Eigen库实现科氏加速度修正的示例代码： ```cpp #include <Eigen/Core> #include <Eigen/Geometry> void covarianceStabilizingCorrection(const Eigen::Vector3d& gyro, const Eigen::Vector3d& acc, double dt, Eigen::Quaterniond& delta_q) { // Compute the correction term Eigen::Vector3d gravity{ 0.0, 0.0, 9.81 }; Eigen::Vector3d corrected_acc = delta_q.conjugate() * acc - gravity; // Compute the angular velocity correction Eigen::Vector3d gyro_correction = corrected_acc.cross(acc) / acc.squaredNorm(); // Compute the delta quaternion from the angular velocity correction Eigen::Quaterniond delta_q_correction; delta_q_correction.w() = 1.0; delta_q_correction.vec() = gyro_correction * dt / 2.0; // Apply the correction to the delta quaternion delta_q *= delta_q_correction; } ``` 这是一个简单的示例，假设已知IMU的角速度（gyro）、加速度（acc）和时间间隔（dt），以及当前的姿态估计（delta_q）。该代码通过计算角速度矫正项，并将其应用于姿态估计的增量四元数（delta_q）来实现科氏加速度修正。

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

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

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值