vins中imu融合_vins-fusion代码解读[五] imu在vins里的理解-程序员宅基地
技术标签： vins中imu融合
SLAM新手，欢迎讨论。
IMU作用
vins中，IMU只读取IMU六轴的信息，3轴线加速度(加速度计)和3轴角速度(陀螺仪)。
通过对陀螺仪的一次积分，直觉上可以获取姿态的变化情况，对加速度的一次积分和二次积分结合陀螺仪的信息，可以获取位置的变换情况。
背景知识：
IMU: 通常6轴陀螺仪，包括3轴线加速度和3轴角速度。9轴陀螺仪增加3轴地磁信息。
MEMS 陀螺仪没有姿态的输出，所有的MEMS陀螺都是非旋转装置，通过获取一个振动机械元件上的哥氏加速度效应，实现角速率检测。即一个验证质量在一个平面内做正弦振动，如果此平面以角速率Ω旋转，那么哥氏力就会使该质量以垂直于该平面的方向做正弦振动，其幅值正比于Ω。对哥氏力所引起的运动进行测量，就可获得Ω的信号，这就是微机械陀螺的基本工作原理。
市面上IMU可以输出姿态四元数或者欧拉角，是通过3轴线加速度和3轴角加速度或者增加3轴地磁信息融合求解出来。
加速度a能够提供重力g在俯仰角和翻滚角平面的投影，但是由于IMU本身在不断运动变化，a不等于g，因此不能很好的反应翻滚角和俯仰角，但是长期来看，这种方法不会产生大的漂移。
角速度w能够预测三个角度的变化量，在短期来看，积分能够预测两个时间段内角度的变化，长期运用会造成角度的漂移。
因此通常的做法是融合两者计算结果(卡尔曼滤波)，能够得到精确的俯仰角和翻滚角，由于g在偏航角的平面上没有投影，因此偏航角是不准确的(因为相当于只能依靠角速度的积分获取)，会有很大的漂移。融合地磁信息之后，可以提高偏航角的准确度，但是同样存在偏移。而且地磁的信息很容易受到环境的干扰，大型设备用电，电机转动都可能产生磁场，干扰地磁信息。
IMU的模型：
以t时刻，IMU作为参考系，我们能够得到以下的IMU模型。
左边是我们得到的测量值，包括了3轴的加速度，以及3轴的角速度信息。右边第一项是陀螺仪和加速度计的真值，第二项是偏移偏移，最后一项是测量噪声项。注意一点的是，由于IMU测量的时候，侧的是除了重力加速度外，所有外力对IMU参考系力的和对系统产生的加速度，会把反向重力加速度测量在内。意思是，当IMU自由落体的时候，加速度的读数是0。IMU平放在桌子上的时候，读数是g。
这里假设噪声na,nb是服从高斯正态分布，而ba,bw偏置服从随机游走模型(倒数服从高斯正态分布)。
因此，我们通过这个IMU模型，可以获取得到任意时刻的位置，速度，以及姿态。即，我们在已知初始的P1,Q1,V1,ba1,bg1的情况下，并且知道每个时刻ba,bg的时候，通过下面的这个式子的离散化表示，我们就能够通过积分得到后面这每一个时刻的P,Q,V。
即可以通过这个式子：
正常这个公式是可以比较直观上理解的。但是有一个问题，因为这个公式中，有一个R旋转矩阵是从imu坐标系旋转到世界坐标系的绝对位姿。由于我们后端优化的过程中，会不断的修正p,q,v的值，使得一个代价函数最小，现在我们一旦修改了一个p,因为积分项里面有一项与IMU的绝对位姿有关，因此我们又要进行重复的一个积分运算。
即：
第二帧位姿发生了改变，导致最终从第二帧开始，所有的积分项都要重新进行积分，非常耗时。
为了避免这一个重复积分运算的过程，vins提出了预积分的概念，通过了坐标系的转换，将绝对的坐标姿态转化为了相对的坐标转换，把要积分的项转化了一个形式。即把R的绝对位姿转化成相对位姿，相对于每一帧的绝对位姿，即：
现在三个临时量只和当前的加速度计偏移和陀螺仪的偏移有关。而于他们在世界坐标系的绝对位姿没有任何关系。任何帧的P，V，Q的改变不影响积分过程，因此就不用一直重复积分。
这三个变量只和偏移ba,bw有关。更新偏移，由于偏移变化是随时间缓慢变化的，因此可以用泰勒一阶展开。
而通过离散积分的方式，可以获取这三个积分项的离散迭代表达：
到这里，我们就可以带入IMU 6个轴的观测值，进行迭代求解，最终解算出这三个积分项。
到这里，我们还不知道这三个积分项的含义是什么，直观上理解，可以认为，bk帧做自由落体运动，以这个帧作为参考系，imu在bk到bk+1这段时间的位移，速度，姿态的变化。从数学上去描述就是：
因此imu的残差vins就定义为：
残差是一个向量，最终的残差是一个标量，因此我们就要把向量转化为一个标量。即要乘以他的协方差矩阵。协方差矩阵可以理解为，因为向量中每个元素变化的情况值不一样，有些元素变化得比较大(方差大)，有些元素变化比较小(方差小)，为了使得把这种变化统一起来，因此需要有一个标准来量化它，就认为是这个协方差矩阵。为了获取协方差矩阵，需要建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。
噪声对角线协方差矩阵就是我们需要标定imu的四个参数。两个测量噪声的方差，两个偏置倒数的方差。
总结预积分：
为了使得非线性优化中，不用每次改变位姿后，积分需要重新计算的问题。
现在改变位姿(速度，位移，旋转四元数关于世界坐标系的)，预积分内的项保持不变。
改变ba,bw后，预积分的项可以通过泰勒一阶展开来进行更新。
智能推荐
http隧道 java_使用java语言实现http隧道技术-程序员宅基地
文章浏览阅读119次。该楼层疑似违规已被系统折叠隐藏此楼查看此楼/***Getaparametervalue**@paramkeyString*@paramdefString*@returnString*/publicStringgetParameter(Stringkey,Stringdef){returnisStandalone?System.getProperty(ke..._java http隧道
Keepalived高可用+邮件告警_keepalived sendmail-程序员宅基地
文章浏览阅读913次。IP主机名备注192.168.117.14keepalived-master主节点192.168.117.15keepalived-slaver备节点192.168.117.100VIP1.主备节点均安装keepalived# yum install -y keepalived httpd2.主备节点均修改keepalived日志存放路径..._keepalived sendmail
SPFILE 错误导致数据库无法启动(ORA-01565)_ora01565 ora27046-程序员宅基地
文章浏览阅读469次。--==========================================--SPFILE错误导致数据库无法启动(ORA-01565)--========================================== SPFILE错误导致数据库无法启动 SQL> startup ORA-01078: failurein proce_ora01565 ora27046
功能测试基础知识（1）-程序员宅基地
文章浏览阅读6.1k次，点赞2次，收藏54次。功能测试基础知识总结_功能测试
postgresql 中文排序_pg中文排序-程序员宅基地
文章浏览阅读3.2k次，点赞3次，收藏2次。pg 中文首字母排序_pg中文排序
[Mysql] CONVERT函数_mysql convert-程序员宅基地
文章浏览阅读3.1w次，点赞23次，收藏109次。本文主要讲解CONVERT函数_mysql convert
随便推点
HTML5与微信开发（2）-视频播放事件及API属性_微信开发者工具视频快进-程序员宅基地
文章浏览阅读8.6k次，点赞2次，收藏2次。HTML5 的视频播放事件想必大家已经期待很久了吧，在HTML4.1、4.0之前我们如果在网页上播放视频无外乎两种方法： 第一种：安装FLASH插件或者微软发布的插件 第二种：在本地安装播放器，在线播放组件之类的 因为并不是所有的浏览器都安装了FLASH插件，就算安装也不一定所有的都能安装成功。像苹果系统就是默认禁用FLASH的，安卓虽然一开始的时候支持FLASH，但是在安卓4.0以后也开始不_微信开发者工具视频快进
JedisConnectionException Connection Reset_jedisconnectionexception: java.net.socketexception-程序员宅基地
文章浏览阅读5.4k次，点赞3次，收藏4次。在使用redis的过程常见错误总结1.JedisConnectionException Connection Reset参考这边文章：Connection reset原因分析和解决方案https://blog.csdn.net/cwclw/article/details/527971311.1问题描述Exception in thread "main" redis.clients...._jedisconnectionexception: java.net.socketexception: connection reset
Lua5.3版GC机制理解_lua5.3 gc-程序员宅基地
文章浏览阅读8.3k次，点赞8次，收藏42次。目录1.Lua垃圾回收算法原理简述2.Lua垃圾回收中的三种颜色3.Lua垃圾回收详细过程4.步骤源码详解4.1新建对象阶段4.2触发条件4.3 GC函数状态机4.4标记阶段4.5清除阶段5.总结参考资料lua垃圾回收(Garbage Collect)是lua中一个比较重要的部分。由于lua源码版本变迁，目前大多数有关这个方面的文章都还是基于lua5.1版本，有一定的滞后性。因此本文通过参考当前..._lua5.3 gc
手机能打开的表白代码_能远程打开，各种手机电脑进行监控操作，最新黑科技...-程序员宅基地
文章浏览阅读511次。最近家中的潮人，老妈闲着没事干，开始学玩电脑，引起他的各种好奇心。如看看新闻，上上微信或做做其他的事情。但意料之中的是电脑上会莫名出现各种问题？不翼而飞的图标？照片又不见了？文件被删了，卡机或者黑屏，无声音了，等等问题。常常让她束手无策，求助于我，可惜在电话中说不清，往往只能苦等我回家后才能解决，那种开心乐趣一下子消失了。想想，这样也不是办法啊， 于是，我潜心寻找了两款优秀的远程控制软件。两款软件...
成功Ubuntu18.04 ROS melodic安装Cartograhper+Ceres1.13.0，以及错误总结_ros18.04 安装ca-程序员宅基地
文章浏览阅读1.8k次。二.初始化工作空间三.设置下载地址四.下载功能包此处可能会报错，请看：rosdep update遇到ERROR: error loading sources list: The read operation timed out问题_DD᭄ꦿng的博客-程序员宅基地接下来一次安装所有功能包，注意对应ROS版本 五.编译功能包isolated：单独编译各个功能包，每个功能包之间不产生依赖。编译过程时间比较长，可能需要几分钟时间。此处可能会报错：缺少absl依赖包_ros18.04 安装ca
Harbor2.2.1配置（trivy扫描器、镜像签名）_init error: db error: failed to download vulnerabi-程序员宅基地
文章浏览阅读4.1k次，点赞3次，收藏7次。Haobor2.2.1配置（trivy扫描器、镜像签名）docker-compose下载https://github.com/docker/compose/releases安装cp docker-compose /usr/local/binchmod +x /usr/local/bin/docker-composeharbor下载https://github.com/goharbor/harbor/releases解压tar xf xxx.tgx配置harbor根下建立:mkd_init error: db error: failed to download vulnerability db: database download