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后，预积分的项可以通过泰勒一阶展开来进行更新。
智能推荐
LLVM 编译器学习笔记之三十六-- 指令调度Instruction scheduling_llvm编译器指令-程序员宅基地
文章浏览阅读3.2k次，点赞4次，收藏9次。1、参考Instruction scheduling in LLVM - 知乎，在中后端均存在指令调度指令调度模型2、在llvm12->llvm14 中Machine Instruction Scheduler有个patch增强了load/store指令的合并，也就是指令调度不仅仅只是改变指令的顺序，该优化类似gcc中的store-merge优化....................................................................._llvm编译器指令
没想到你们是这种人！-程序员宅基地
文章浏览阅读4.1k次。大家好，我是北妈0.从我3月初决定开启朋友圈每天碎碎念以来，每天坚定不移的至少发一条，一段每天感悟和金句。作为总结，也把它当成一个日记给自己和好友，甚至是未来的孩子去回顾，如果有可能随着时...
zhaowei -学习猜拳游戏_机器学习 划拳游戏-程序员宅基地
文章浏览阅读218次。import random# 电脑出拳 电脑产生一个1-3的随机数computer = random.randint(1, 3) # 数字# print(computer)# print(computer)# 我们出拳our = int(input("请出拳（1石头 2 布 3剪刀）：")) # 字符串# print(our)# 判断谁输谁赢 orif computer ..._机器学习 划拳游戏
2022年编程语言热度排行榜来啦，快来看看你学习的语言排第几_2022年编程语言排行榜-程序员宅基地
文章浏览阅读4.9k次。一直以来，编程语言都是程序员非常关注的话题。年末将至，是否会有程序员发出疑问——“2022 年行业需求最大的编程语言，花落谁家?”从 2021 年 10 月到 2022 年 11 月，DevJobsScanner 分析了超过 1200 万个开发人员职位。从这 1200 万份工作中，DevJobsScanner 挑选了明确需要编程语言的工作岗位。现在，一起回顾 2022 年行业需求最大的 8 种编程语言。_2022年编程语言排行榜
开发日志[1] 错误：未能找到引用的组件“System.Windows.Forms.DataVisualization”_system.windows.forms.datavisualization netframewor-程序员宅基地
文章浏览阅读696次。错误：未能找到引用的组件“System.Windows.Forms.DataVisualization”原因：代码中是MSChart表格插件未在当前系统中安装。解决：下载对应版本的MSChart.exe安装程序，安装后再次在项目中添加引用即可。相关：......_system.windows.forms.datavisualization netframework 3.5.1
一阶跃函数c语言程序设计教程课后答案,最新整理c语言上机操作题参考.doc-程序员宅基地
文章浏览阅读628次。最新整理c语言上机操作题参考实验一 上机操作初步(2学时)一、实验方式：一人一机二、实验目的：1、了解C语言程序的特点与基本结构。2、了解C语言程序的编译环境。3、掌握使用C语言编写和调试程序的流程：编辑-保存-编译-构建(组件)-执行。4、掌握C语言程序在Visual C++ 6.0环境下的调试过程。5、初步了解C程序的特点。三、实验内容：说明：前三题为必做题目，第四题为选做题目。1、编辑实验..._编写程序,通过输入x的值,计算阶跃函数y的值
随便推点
MacOS 系统成功安装 tensorflow 步骤_mac装tensorflow-程序员宅基地
文章浏览阅读1.3k次，点赞21次，收藏18次。tensorflow 2 mac os 系统安装 步骤_mac装tensorflow
ES（Elasticsearch）7.6.1安装教程_安装elasticsearch-程序员宅基地
文章浏览阅读5.8k次，点赞4次，收藏21次。ES 安装教程注意：除了最后启动ES的时候在es账户下执行，其他都是在root 下执行的。⼀、前期准备java 11 的安装，请大家自行安装。elasticsearch启动不能以root⽤户运⾏，所以创建es⽤户（root⽤户状态下操作）useradd espasswd es然后输入两次：zong2015如果需要删除用户userdel -r es将elasticsearch⽤户添加到sudoersvim /etc/sudoers..._安装elasticsearch
spring boot>>RabbitMQ中间件发送验证码_basevo依赖-程序员宅基地
文章浏览阅读2.1k次。起因：短信发送的应用场景非常多，在较大的系统中，短信一般作为单独的服务独立运行，而短信发送任务的触发基本有两种方式。1、定时获取Redis中短信发送任务，2、消息中间件订阅短息任务队列。RabbitMQ订阅短信：短信服务：短信服务为独立工程。将阿里云 SmsServer抽取为工具类。1、pom依赖： <!--RabbitMQ-->&..._basevo依赖
uiautomatorviewer拉取手机竖屏却显示为横屏的问题_uiautomatorviewer方向倒了-程序员宅基地
文章浏览阅读2.4k次，点赞3次，收藏3次。uiautomatoruiautomatorviewer拉取手机竖屏却显示为横屏的问题拉取的页面显示解决方法将保存下来的图片进行旋转，旋转为竖屏显示并保存点击open，导入图片和uix格式文件显示正常uiautomatorviewer拉取手机竖屏却显示为横屏的问题拉取的页面显示解决方法点击save将保存下来的图片进行旋转，旋转为竖屏显示并保存点击open，导入图片和uix格式文..._uiautomatorviewer方向倒了
加密技术简介-程序员宅基地
文章浏览阅读1.3w次，点赞7次，收藏32次。参考文章：信息加密技术简介 [浅谈加密技术]（https://blog.csdn.net/fanyunda1988/article/details/50897216）加解密简介加解密分类常见的加密技术介绍其余问题，有关于AES 和 RSA 加密技术，对称加密，非对称加密。优点和缺点，以及API会用， 加密的原理。涉及到位运算。让你走一个算法的话你会怎么走？ HTTP 和 H..._加密技术
使用迭代器Iterator遍历Collection_.keyset().iterator().next()-程序员宅基地
文章浏览阅读1.1k次。实现LRU算法时，发现使用java的LinkedHashMap时如果删除队头元素（最长时间未访问的元素），需要coll.remove(coll.keySet().iterator().next());因为Map中元素无序，根据key的hashCode（）排序，而LinkedHashMap虽然有序，但是无法获取队头和队尾元素，所以只能采用迭代器模式。java.utils包下定义的迭代器接口：Iterator。Iterator对象称为迭代器(设计模式的一种)，主要用于遍历 Collection 集合中的_.keyset().iterator().next()