程序员经验分享-hwhale

IMU预积分及残差雅克比计算

2023-05-16

    前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式,收获很大,再看到VIORB的时候,大都用的这一套公式.

    后再看VINS的时候,VINS用的是连续时间的预积分,看一些大佬的博客,说代码里写的是离散时间的.这里将所看到的做一下整理.

  要求的是:

                    

即要求

            

   这里要求的三个临时状态量只与IMU的偏置有关系，而与其他状态无关 可以写成一阶线性展开近似

      

  其中的

             

          

论文中用的欧拉积分,代码中用的中值积分,中值积分求法如下:

首先陀螺仪的测量模型和加速度计的测量模型:

    

     离散时间的预积分过程(中值法)

        (1)  (2)  (3)

                  由泰勒展开式可得 

                           \delta \theta _{k+1}=\delta \theta _{k}+\dot{\delta \theta _{k}}\delta t                  (4)

                   由Quaternion kinematics for the error-state KF第54页(237c)公式:

                                  (5)

                 将(1)代入到(5)得(注意这里w_{m}-w_{b}实际上就是w_{t}+w_{n}而w_{n}在后面的处理,其实就是w_{t}):

                                (6)

                 将(6)代入到(4)得:

                           (7)

                  得到了\delta \theta _{k+1}再来看\delta v_{k+1}.

                    \delta \beta _{k+1}=\delta \beta _{k}+\dot{\delta \beta _{k}}\delta t=\delta \beta _{k}+{a}'\delta t      (8)  (注意这里,{a}'不是a的导数,就是公式3)

                  由Quaternion kinematics for the error-state KF第54页(237b)公式:

                             (9)

                  将(3)代入到(9)可以得:

                        \dot{\delta v_{k+1}}=-\frac{1}{2}q_{k}(a_{k}-b_{ak})\delta \theta _{k}-\frac{1}{2}q_{k+1}(a_{k+1}-b_{ak})\delta \theta _{k+1}-\frac{1}{2}q_{k}\delta b_{ak}-\frac{1}{2}q_{k+1}\delta b_{ak}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1} (10)  没有\delta g项

                   将(7)代入到(10)可得:

                        (11)

                     同理可得到位移的关系:

                (12)

                          然后可以得到陀螺仪和加速度计偏置的误差:

                                  (13)

                                  (14)

                            综上可以写成矩阵形式:

                  

                      其中:

                       

                矩阵可简写为:

             

              

             以上所求雅克比为预积分的雅克比

           求完

                 

   

还需要求针对偏置的偏导项,在优化时用到做残差优化.

          

  偏导项部分可以从上面的矩阵形式直接看出.

    以上有看完代码后的修改...有很多很糙的截图.

 

             还需要计算残差的雅克比矩阵. 残差的雅克比是用来优化用的.

             残差就是将预积分相当于测量值（就是真值，因为没有比这个更准确的值了，那当然就是真值了），要估计的状态就是估计值，所以预积分测量值减去状态估计值就是残差，在后面会提到需要估计的IMU估计值有p,v,q,ba,bg。P和q的估计值初始值比较好得到（和视觉相关，可以直接用视觉的初值），而v,ba,bg这三个量的估计值初始值就比较难得到了，因为视觉没有这三个初始量，就会用到下面的联合初始化得到初始的这三个量.       

             残差向量 r_{\Delta R_{ij}}, r_{\Delta v_{ij}}, r_{\Delta P_{ij}},状态向量和偏置\delta b_{a_{k}},\delta b_{a_{k+1}},\delta b_{w_{k}},\delta b_{w_{k+1}}.

             残差可以表示为:

             

           首先看r_{\Delta R_{ij}}

1.    对Ri求偏导.

         

2. 对Rj求偏导.

       2\left [ q_{b_{k+1}}^{w}^{-1}\otimes \left [ 0,I/2 \right ]^{T}\otimes q_{b_{k}}^{w}\otimes \check{r}_{b_{k+1}}^{b_{k}} \right ] 

3. 对偏置\delta b_{w_{k}}求偏导.

       2\left [ q_{b_{k+1}}^{w}^{-1}\otimes q_{b_{k}}^{w}\otimes \check{r}_{b_{k+1}}^{b_{k}} \otimes\left [ 0,J_{b_{w}}^{r}/2 \right ]^{T} \right ]

     然后是对r_{\Delta v_{ij}}求雅克比

   1.   对\delta b_{w_{k}}求偏导.

             -J_{b_{w}}^{\beta }

   2.   对\delta b_{a_{k}}求偏导.

             -J_{b_{a}}^{\beta }

    3.   对v_{b_{k}}求偏导.

         

    4.   对v_{b_{k+1}}求偏导.

           q_{w}^{b_{k}}

     5.   对Ri求偏导.

         

              

     最后r_{\Delta R_{ij}}求雅克比

   1.  对 \delta b_{w_{k}}求偏导

         -J_{b_{w}}^{P}

   2. 对\delta b_{a_{k}}求偏导

        -J_{b_{a}}^{P}

    3. 对P_{b_{k}}求偏导

       

    4. 对P_{b_{k+1}}求偏导

        

     5. 对v_{b_{k}}求偏导

         

      6. 对Ri求偏导

          

       

    全部的残差雅克比求完了.

参考: 五行缺帅wangshuailpp博客

          VINS-Mono代码分析总结

          邱笑晨博士的IMU预积分推导

          游振兴大佬的四元数运动学笔记（1）旋转的表示

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

IMU

预积分及残差雅克比计算

IMU预积分及残差雅克比计算 的相关文章

  • IMU预积分学习

    IMU预积分学习 一 IMU状态传递方程 一 IMU状态传递方程 几种不同的表达形式 xff1a 1 lio mapping xff1a 参考大佬 xff1a lio mapping 及 VINS Mono代码及理论推导 xff08 2 x

  • VIO标定工具kalibr和imu_utils的使用

    0 参考资料 Kalibr进行IMU 43 相机的标定 xff1a 这个步骤写的非常好 xff0c 应该是目前看到的最符合的步骤了 使用ROS功能包标定相机内参 Kalibr标定camera IMU详细步骤 xff1a 这篇博客里给出了它的

  • ROS2手写接收IMU数据(Imu)代码并发布

    目录 前言接收IMU数据IMU的串口连接问题 python接收串口数据 python解析数据ROS2发布IMU数据可视化IMU数据效果 前言 在前面测试完了单独用激光雷达建图之后 xff0c 一直想把IMU的数据融合进去 xff0c 由于经

  • 从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导

    本节学习IMU预积分 xff0c 推导离散时间下的IMU预积分公式 xff0c 并解读相应代码 VINS Mono Fusion代码学习系列 xff1a 从零学习VINS Mono Fusion源代码 xff08 一 xff09 xff1a

  • realsense D435i双目IMU 数据集

    realsense D435i 双目IMU数据集 使用双目 43 IMU的数据双目内参双目IMU外参 使用双目 43 IMU的数据 双目内参 model type PINHOLE camera name camera image width

  • Kalibr 之 Camera-IMU 标定 (总结)

    Overview 欢迎访问 持续更新 xff1a https cgabc xyz posts db22c2e6 ethz asl kalibr is a toolbox that solves the following calibrati

  • 多IMU车载GNSS/INS数据集及介绍

    文章目录 前言非常棒的GINS数据集采集轨迹采集时间MEMS IMU详细信息IMU测量值方程及相关误差建模参考噪声参数 安装参数文件格式描述 前言 本文作者感谢武汉大学多源智能导航实验室和牛小骥教授提供了本文所需的GNSS INS数据集 本

  • 使用IMU进行状态估计及进阶

    文章目录 前言基本思想一 姿态估计1 1 通过6轴IMU来进行姿态估计的入门级方法1 1 1 通过加速度计计算姿态1 1 2 引入陀螺仪来得到更好的姿态估计 1 2 四元数解算姿态角解析 二 姿态估算与滤波的关系2 1 状态方程和观测方程2

  • 关于imu的介绍

    1 imu时惯性运动丹云 xff0c 包含加速度计和陀螺传感器的组合 它被用来检查加速度和角速度 xff08 IMU传感器 xff0c 你所需要知道的全部 知乎 xff09 虽然时外文翻译的 xff0c 凡是整体风格清晰 2 imu的使用

  • 在ROS下Intel RealSense D435i 驱动的安装，避免踩坑，避免缺少imu话题等各种问题（适用于D400系列、SR300和T265跟踪模块等）

    版权声明 本文为博主原创文章 未经博主允许不得转载 https blog csdn net AnChenliang 1002 article details 109454465 目录 背景 方法1 使用apt安装 不建议使用此方法 了解一下

  • 再谈IMU数据处理（滤波器）

    本文开始前 xff0c 先回答一个问题 上一篇文章最后提到了卡尔曼滤波器用来做一维数据的数字滤波处理 xff0c 最终的实验结果说 xff1a 该模型下的卡尔曼滤波处理与二阶IIR低通滤波处理效果几乎一致 有网友指出是错误的 xff0c 卡

  • RealSenseD345I —— imu + camera标定

    目录 1 标定目的 2 标定准备 3 标定步骤 nbsp nbsp nbsp nbsp 1 IMU标定 nbsp nbsp nbsp

  • Xsens Mti-g-710 IMU driver在Ubuntu18.04 ROS melodic中的安装使用

    Ubuntu18 04下安装的ROS melodic 如何使用Xsens Mti g 710 IMU driver xff1f 这里给出一个详细步骤说明 这里的IMU是USB接口 1安装 首先插入IMU的USB口 命令行运行 gt lsus

  • IMU校正以及姿态融合

    本文为博主 声时刻 原创文章 xff0c 未经博主允许不得转载 联系方式 xff1a shenshikexmu 64 163 com 缘起 有缘在简极科技兼职两年 接触了IMU xff0c 我去的时候那家公司还是一个要把IMU放进足球的公司

  • 星网宇达（惯导+IMU）设备实现自动采点

    一 创建和打开gps Road txt文件 xff0c 准备往里写数据 FILE span class token operator span p span class token operator 61 span span class t

  • IMU误差模型简介及VINS使用说明

    1 IMU误差来源 2 IMU噪声模型 Noise and Bias kalibr中的imu noise model 参考 xff1a https github com ethz asl kalibr wiki IMU Noise Mode

  • 利用 imu_utils 标定 imu

    目录 前言 一 安装 imu utils 二 编译出现的错误 三 操作步骤 四 结果 前言 记录利用 imu utils 标定 imu xff0c 最近在使用imu做导航 xff0c 要对imu进行标定 xff0c 使用标定得到的加速度的噪

  • 【深蓝学院】手写VIO第2章--IMU传感器--笔记

    0 内容 1 旋转运动学 角速度的推导 xff1a 左 61 omega wedge xff0c 而

  • 浙江大学提出自感知IMU网络精准捕获3D变形

    运动捕捉目前有两种主流方法 视觉捕捉能捕获复杂的三维几何变形 但依赖于昂贵的光学设备并且存在视线遮挡问题 基于IMU的方法虽然简便 但难以捕获细微的3D变形 为了解决这个问题 浙江大学的研究者们提出了一种可配置的自感知IMU传感器网络 解决

  • An Introduction for IMU 2 - IMU数据融合与姿态解算

    在上一篇博客中 我们已经介绍了IMU的内部工作原理 以及如何通过Arduino读取MPU6050的数据 虽然可以从DMP直接读取姿态角 但其数据返回的频率相对较低 同时由于DMP库不是开源的 其内部的工作原理 输出姿态角的准确性都不清楚 而

随机推荐

  • 素数伴侣 java

    span class token keyword package span span class token namespace com span class token punctuation span patience span cla

  • 分布式系统概念和设计-分布式文件系统服务体系结构和实践经验

    分布式系统概念和设计 文件系统的特点 负责文件的组织 xff0c 存储 xff0c 检索 xff0c 命名 xff0c 共享和保护 文件包含数据和属性 数据 xff1a 包含一系列数据项 8比特的字节 xff0c 读写操作可访问任何一部分数

  • ROS(C++)中的subscribe使用

    subscribe函数用于订阅topic并添加回调函数 比如 xff1a cloud subs span class token operator 61 span nh span class token punctuation span s

  • 基于人工标识的机器人定位方法

    基于人工标识的机器人定位方法 qquad 在机器人视觉导航任务中 xff0c 常常需要通过识别人工标识获得机器人在世界坐标系中的位姿 它的原理是通过匹配人工标识中特征点的世界坐标和图像坐标 xff0c 从而估算机器人坐标系与世界坐标系 之间

  • Momenta电话面试笔记

  • Snipaste贴图软件使用方法及快捷键

    文章目录 snipaste介绍1 截图2 标注3 贴图 snipaste是一款专业的免费截图软件 它帮助用户轻松解决截图 拼图 图像编辑等多种问题 xff0c 是最受用户喜欢的截图工具之一 snipaste介绍 Snipaste是一款免费安

  • kubernetes 如何发音？

    原文链接地址 xff1a 阳明的博客 有很多人不知道kubernetes应该怎么发音 xff0c 包括我之前也读错了 xff0c 正确的发音是 kub netis xff0c 重音在第三个音节 xff0c 读音 xff1a 库伯耐踢死 xf

  • sonic如何写ACL

    一 目前ACL配置需要用到openconfig json格式去操作 xff1b ACL full update an ACL rule definition file will be provided in openconfig json

  • 最近学习的德雷福斯模型

    德雷福斯模型 技术领域来讲 xff1a 个人对某种技能的掌握程度分为5个阶段 阶段一 xff1a 新手 新手在使用某种技能的人群中占比小于10 他们没有或只有很少的经验 xff0c 不知道自己的行为是对是错 xff0c 需要有人指导或者参考

  • 串级PID

    文章来源 xff1a 到底什么是串级PID xff1f 什么是串级PID 什么是串级PID xff1f 顾名思义就是两个串起来的PID xff0c 下面是一个双闭环的例子 xff0c 外环是位置环 xff0c 内环是速度环 xff0c 最终

  • 嵌入式工程师应对中年危机的上中下策

    文章来源 xff1a 电子通信 xff0c 嵌入式工程师应对中年危机的上中下策 之前火哥有一篇为什么电子通信 xff0c 嵌入式工程师会有中年危机的文章 xff0c 给出了一些中年危机的原因分析 xff0c 但是并没有给意见 这篇文章我们继

  • Action 详解

    回到首页 Action 是用于处理请求操作的 xff0c 它是由 StrutsPrepareAndExecuteFilter 分发过来的 在 Struts2 框架中 xff0c Action 是框架的核心类 xff0c 被称为业务逻辑控制器

  • ADRC学习笔记( 一)

    近期正在自学自抗扰技术 xff0c 不得不感叹韩老师乃神人也 xff01 话不多说先把这两天的学习成果与大家分享 xff0c 后续在做详细的介绍 欢迎大家批评指正 xff08 1 xff09 过渡过程 输入一个阶跃信号 xff0c 其跟踪效

  • ADRC学习笔记（二）

    添加链接描述源码1 最速跟踪微分器TD 它的离散表达式为 xff1a 参数中 xff1a V t 是目标值h h0为积分步长 xff0c 一般来说h可以等于h0 xff0c 但是为了减少超调和减少震荡 xff0c 才把他们分开 xff0c

  • 基于LQR的一阶倒立摆控制仿真

    1 LQR简介 LQR linear quadratic regulator 即线性二次型调节器 xff0c 其对象是现代理论中以状态空间形式给出的线性系统 LQR最优设计是指设计出的状态反馈控制器 K要使二次型目标函数J 取最小值 而 K

  • 神经网络中Batch和Epoch之间的区别

    来自蜂口知道公众号 随机梯度下降法是一种具有大量超参数的学习算法 通常会使初学者感到困惑的两个超参数 Batch大小和Epoch数量 它们都是整数值 xff0c 看起来做的事情是一样的 在这篇文章中 xff0c 您将发现随机梯度下降中Bat

  • dubbo+springMVC消费端报错classNotFound

    首先 xff0c 说明一下我的错误原因 xff1a 是因为provider端打jar包方式的错误 导致的 xff0c 如果你已经将这种错误可能排除了 xff0c 就不用浪费时间看我啰嗦了 maven基础比较弱 xff0c 之后系统学一下吧

  • kubernetes HPA-超详细中文官方文档

    HPA全称是Horizontal Pod Autoscaler xff0c 翻译成中文是POD水平自动伸缩 xff0c 以下都会用HPA代替Horizontal Pod Autoscaler xff0c HPA可以基于CPU利用率对repl

  • openstack安装和使用

    前言 本篇文章会介绍openstack的安装 openstack各组件的配置和功能 还会演示openstack云主机的创建流程 xff0c 整篇文章花费一天时间完成 xff0c 万字长文 xff0c 需要的可以关注 xff0c 相互学习 x

  • IMU预积分及残差雅克比计算

    前段时间推了泡泡机器人邱笑晨博士的IMU预积分公式 收获很大 再看到VIORB的时候 大都用的这一套公式 后再看VINS的时候 VINS用的是连续时间的预积分 看一些大佬的博客 说代码里写的是离散时间的 这里将所看到的做一下整理 要求的是

热门标签