IMU预积分【SLAM】

前言

预积分的推导过程比较多,所以这里只记录关键结论。
其实这些公式不太好记忆,因为预积分推导过程的想法来源很巧妙,无法看出物理意义。

预积分定义式(必须记住)

一切推导的来源:
最好记忆的旋转相对运动量:
△ R i j ≐ R i T R j = ∏ k = i j − 1 E x p ( ( ω ~ k − b g , k − η g d , 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) RijRiTRj=k=ij1Exp((ω~kbg,kηgd,k)t)
△ v i j ≐ R i T ( v j − v i − g △ t i j ) = ∑ k = i j − 1 △ R i k ( a ~ k − b a , k − η a d , k ) △ t \triangle v_{ij} \doteq R_{i}^{T}(v_{j} - v_{i} -g\triangle t_{ij}) = \sum_{k=i}^{j-1}\triangle R_{ik}(\tilde{a}_{k}-b_{a,k}-\eta_{ad, k})\triangle t vijRiT(vjvigtij)=k=ij1Rik(a~kba,kηad,k)t
△ p i j ≐ R i T ( p j − p i − v i △ t i j − 1 2 g △ t i j 2 ) = ∑ k = i j − 1 [ △ v i k △ t + 1 2 △ R i k ( a ~ k − b a , k − η a d , k ) △ t 2 ] \triangle p_{ij} \doteq R_{i}^{T} \left ( p_{j} - p_{i} - v_{i}\triangle t_{ij}-\frac{1}{2}g\triangle t_{ij}^{2} \right ) = \sum_{k=i}^{j-1} \left[ \triangle v_{ik} \triangle t +\frac{1}{2}\triangle R_{ik} \left( \tilde{a}_{k} - b_{a,k} -\eta_{ad, k} \right) \triangle t^{2} \right] pijRiT(pjpivitij21gtij2)=k=ij1[vikt+21Rik(a~kba,kηad,k)t2]

η g d , k \eta_{gd,k} ηgd,k η a d , k \eta_{ad,k} ηad,k是离散化后的随机游走噪声(即建模为零均值白噪声高斯过程的测量噪声,因为也是角度或速度的随机游走所以才会这样描述)。

预积分的意义

IMU本身的数学模型中包括测量方程和噪声模型,再加上IMU运动学方程(本质上是微分方程组),可以推导出IMU在 △ t \triangle t t时间段内的直接积分公式,在滤波方法中可以用直接积分作为预测,但是在优化方法中,我们并不希望优化过程随着IMU数据进行调用,这样做太浪费计算资源,我们更希望将IMU数据测量值组合在一起处理,所以累积的IMU观测就是所谓的IMU预积分。

直接积分的缺点是,它描述的过程和状态量有关,如果对 i i i时刻的状态进行优化,那么随后的所有IMU时刻的状态也会变化,整个积分要重新计算。

而通过预积分的定义可以看到,预积分定义最右侧公式与状态量无关(除了零偏,零偏发生变化时有单独的更新方法),所以当状态量发生变化时,也无需重新计算预积分量,更不必计算各个IMU时刻的状态量。此外预积分是累加或累乘的形式,给计算层面带来很大便利。

预积分的测量模型和噪声模型

推导过程假设了预积分过程中**零偏固定不变(所以下面公式中是 b g , i b_{g,i} bg,i b a , i b_{a,i} ba,i)**进行处理,实际零偏发生变化时预积分有单独的更新方式。

旋转部分

预积分旋转观测量(记住):
△ R ~ i j ≐ ∏ k = i j − 1 E x p ( ( ω ~ k − b g , i ) △ t ) \triangle \tilde{R}_{ij} \doteq \prod_{k=i}^{j-1}Exp((\tilde{\omega}_{k} - b_{g,i})\triangle t) R~ijk=ij1Exp((ω~kbg,i)t)

旋转噪声项:
E x p ( − δ ϕ i j ) ≐ ∏ k = i j − 1 E x p ( − △ R ~ k + 1 , j T J r , k η g d , k △ t ) Exp(-\delta\phi_{ij}) \doteq \prod_{k=i}^{j-1}Exp(-\triangle\tilde{R}_{k+1,j}^{T}J_{r,k}\eta_{gd,k}\triangle t) Exp(δϕij)k=ij1Exp(R~k+1,jTJr,kηgd,kt)
J r , k J_{r,k} Jr,k由BCH公式引入,和 ω k \omega_{k} ωk b g , k b_{g,k} bg,k有关(假设条件使得 b g , k = b g , i b_{g,k} = b_{g,i} bg,k=bg,i)。随机变量 δ ϕ i j \delta\phi_{ij} δϕij只和随机变量 η g d \eta_{gd} ηgd有关, η g d \eta_{gd} ηgd是均值为 0 0 0的白噪声,所以 δ ϕ i j \delta\phi_{ij} δϕij均值为 0 0 0

然后有:
△ R i j = △ R ~ i j E x p ( − δ ϕ i j ) \triangle R_{ij} = \triangle \tilde{R}_{ij} Exp(-\delta\phi_{ij}) Rij=R~ijExp(δϕij)

旋转噪声项递推更新公式:
δ ϕ i j = △ R ~ j − 1 T δ ϕ i , j − 1 + J r , j − 1 η g d , j − 1 △ t \delta\phi_{ij} = \triangle \tilde{R}_{j-1}^{T}\delta\phi_{i,j-1} + J_{r,j-1}\eta_{gd,j-1}\triangle t δϕij=R~j1Tδϕi,j1+Jr,j1ηgd,j1t

旋转噪声项的协方差递推公式:
j − 1 j-1 j1时刻 δ ϕ i , j − 1 \delta\phi_{i,j-1} δϕi,j1的协方差为 Σ j − 1 \Sigma_{j-1} Σj1,高斯白噪声 η g d \eta_{gd} ηgd的协方差为 Σ η g d \Sigma_{\eta_{gd}} Σηgd(一般假设随机游走过程中高斯白噪声协方差保持不变,所以不区分 η g d , k \eta_{gd,k} ηgd,k)。
Σ j = △ R ~ j − 1 , j T Σ j − 1 △ R ~ j − 1 , j + J r , j − 1 Σ η g d J r , j − 1 T △ t 2 \Sigma_{j} = \triangle\tilde{R}_{j-1,j}^{T} \Sigma_{j-1} \triangle\tilde{R}_{j-1,j} + J_{r,j-1}\Sigma_{\eta_{gd}} J_{r,j-1}^{T} \triangle t^{2} Σj=R~j1,jTΣj1R~j1,j+Jr,j1ΣηgdJr,j1Tt2

速度部分

预积分速度观测量(记住):
△ v ~ i j ≐ ∑ k = i j − 1 △ R ~ i k ( a ~ k − b a , i ) △ t \triangle \tilde{v}_{ij} \doteq \sum_{k=i}^{j-1}\triangle \tilde{R}_{ik}(\tilde{a}_{k}-b_{a,i})\triangle t v~ijk=ij1R~ik(a~kba,i)t
速度噪声项:
− δ v i j ≐ ∑ k = i j − 1 [ △ R ~ i k ( a ~ k − b a , i ) ∧ δ ϕ i k △ t − △ R ~ i k η a d , k △ t ] -\delta v_{ij} \doteq \sum_{k=i}^{j-1}\left[ \triangle\tilde{R}_{ik}(\tilde{a}_{k}-b_{a,i})^{\wedge}\delta\phi_{ik}\triangle t - \triangle\tilde{R}_{ik}\eta_{ad,k}\triangle t \right] δvijk=ij1[R~ik(a~kba,i)δϕiktR~ikηad,kt]
然后有:
△ v i j = △ v ~ i j − δ v i j \triangle v_{ij} = \triangle \tilde{v}_{ij} - \delta v_{ij} vij=v~ijδvij
速度噪声项递推公式:
δ v i j = δ v i , j − 1 − △ R ~ i , j − 1 ( a ~ j − 1 − b a , i ) ∧ δ ϕ i , j − 1 △ t + △ R ~ i , j − 1 η a d , j − 1 △ t \delta v_{ij} = \delta v_{i,j-1} - \triangle\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^{\wedge}\delta\phi_{i,j-1}\triangle t + \triangle\tilde{R}_{i,j-1}\eta_{ad,j-1}\triangle t δvij=δvi,j1R~i,j1(a~j1ba,i)δϕi,j1t+R~i,j1ηad,j1t

位置(位移)部分

预积分位置(位移)观测量(记住):
△ p ~ i j ≐ ∑ k = i j − 1 [ ( △ v ~ i k △ t ) + 1 2 △ R ~ i k ( a ~ k − b a , i ) △ t 2 ] \triangle \tilde{p}_{ij} \doteq \sum_{k=i}^{j-1}\left[ (\triangle\tilde{v}_{ik}\triangle t)+\frac{1}{2}\triangle \tilde{R}_{ik}(\tilde{a}_{k}-b_{a,i})\triangle t^{2}\right] p~ijk=ij1[(v~ikt)+21R~ik(a~kba,i)t2]
位置(位移)噪声项:
− δ p i j ≐ ∑ k = i j − 1 [ − δ v i k △ t + 1 2 △ R ~ i k ( a ~ k − b a , i ) ∧ δ ϕ i k △ t 2 − 1 2 △ R ~ i k η a d , k △ t 2 ] -\delta p_{ij} \doteq \sum_{k=i}^{j-1}\left[ -\delta v_{ik}\triangle t + \frac{1}{2}\triangle\tilde{R}_{ik}(\tilde{a}_{k}-b_{a,i})^{\wedge} \delta\phi_{ik}\triangle t^{2} - \frac{1}{2}\triangle\tilde{R}_{ik}\eta_{ad,k}\triangle t^{2} \right] δpijk=ij1[δvikt+21R~ik(a~kba,i)δϕikt221R~ikηad,kt2]
然后有:
△ p i j = △ p ~ i j − δ p i j \triangle p_{ij} = \triangle\tilde{p}_{ij} - \delta p_{ij} pij=p~ijδpij
位置(位移)噪声项递推公式:
δ p i j = δ p i , j − 1 + δ v i , j − 1 △ t − 1 2 △ R ~ i , j − 1 ( a ~ j − 1 − b a , i ) ∧ δ ϕ i , j − 1 △ t 2 + 1 2 △ R ~ i , j − 1 η a d , j − 1 △ t 2 \delta p_{ij} = \delta p_{i,j-1} + \delta v_{i,j-1}\triangle t - \frac{1}{2}\triangle\tilde{R}_{i,j-1}(\tilde{a}_{j-1}-b_{a,i})^{\wedge}\delta\phi_{i,j-1}\triangle t^{2} +\frac{1}{2} \triangle\tilde{R}_{i,j-1} \eta_{ad,j-1}\triangle t^{2} δpij=δpi,j1+δvi,j1t21R~i,j1(a~j1ba,i)δϕi,j1t2+21R~i,j1ηad,j1t2

重要说明(重要!!!)

1、首先需要记住的公式均是预积分的定义式和观测量的定义式,其实二者形式几乎是相同的,只是观测量把定义式中的随机变量部分分离出去形成了噪声项。
2、说明一下本人对于预积分应用的理解,类比其他SLAM问题中的后端优化问题模型。可以看到预积分的公式可以写成“观测量=状态量推导的预测量+噪声”的形式,比如 △ R ~ i j = △ E x p ( − δ ϕ i j ) \triangle \tilde{R}_{ij} = \triangle Exp(-\delta\phi_{ij}) R~ij=Exp(δϕij),这个公式左侧的观测量由传感器观测值利用观测量定义式计算,右侧的预测量由本文第一部分中预积分定义式的中间公式计算,这样观察会发现,IMU预积分的数学模型和其他SLAM后端优化模型完全一样,求观测值-预测值的最小二乘问题其实就是求预积分的最大后验估计,而噪声项的协方差就是通过高斯分布的概率密度函数引入到优化问题中,指导着残差在求解优化问题中的比重。如果不采用完全准确的协方差,该模型将不会严格匹配预积分的最大后验估计,但是仍然具有最小二乘的意义,只不过残差在求解优化问题中的权重是自定义决定的。

预积分对于零偏的更新

前面的推导过程中,在观测量的定义部分假设了零偏不变,因此考虑零偏变化时观测量和零偏的关系。思路就是假设预积分观测和零偏之间是线性关系,利用泰勒展开保留一阶项(旋转部分的更新方式稍有不同)。重点在于雅可比矩阵的计算,最后还要把雅可比矩阵写成递推关系便于计算。

实际应用中,当优化后零偏发生变化时,利用递推公式先求雅可比矩阵,然后更新到预积分观测量上去。
公式太多了就不敲了(应该也不用记住)。

预积分残差对状态量的雅可比矩阵

这个公式也不敲了,需要的时候查书就可以了。

总结

在实际应用中,如果要进行IMU预积分,当一帧IMU信息到来时需要做:
1、计算三个预积分观测量
2、计算三个噪声项的协方差矩阵,作为后续图优化的信息矩阵
3、计算预积分观测量对零偏的雅可比矩阵,一共五个。

结束预积分计算后,可以把这些结果取出并在优化过程中使用。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/779632.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

c语言教务成绩管理系统1000+

定制魏:QTWZPW,获取更多源码等 目录 题目 代码主函数 教务信息头文件 题目 编写一个C语言程序,实现一个教务成绩管理系统,至少能够管理30条学生信息。其中: 1)学生信息包括:基本信息和成绩信息。 2)基本信息包括:班级,学号,姓名,性别,专业,普通课程选修数…

C++中的STL简介与string类

目录 STL简介 STL的版本 STL的六大组件 string类 标准库中的string类 string类的常用接口 string类对象对容量的操作 size()函数与length()函数 capacity()函数 capacity的扩容方式 reserve()函数 resize()函数 string类对象的操作 push_back()函数 append()函数 operator()函数…

【01-20】计算机网络基础知识(非常详细)从零基础入门到精通,看完这一篇就够了

【01-20】计算机网络基础知识(非常详细)从零基础入门到精通,看完这一篇就够了 以下是本文参考的资料 欢迎大家查收原版 本版本仅作个人笔记使用1、OSI 的七层模型分别是?各自的功能是什么?2、说一下一次完整的HTTP请求…

1688中国站按关键字搜索工厂数据 API

公共参数 名称类型必须描述keyString是申请免费调用key(必须以GET方式拼接在URL中)secretString是调用密钥api_nameString是API接口名称(包括在请求地址中)[item_search,item_get,item_search_shop等]cacheString否[yes,no]默认y…

记录何凯明在MIT的第一堂课:神经网络发展史

https://www.youtube.com/watch?vZ5qJ9IxSuKo 目录 表征学习 主要特点: 方法和技术: LeNet 全连接层​ 主要特点: 主要特点: 网络结构: AlexNet 主要特点: 网络结构: Sigmoid Re…

经典永不过时 Wordpress模板主题

经得住时间考验的模板,才是经典模板,带得来客户的网站,才叫NB网站。 https://www.jianzhanpress.com/?p2484

最详细爬虫零基础教程12——某网评论爬取教程

文章目录 前言一、单页爬取二、翻页案例 前言 本文我们主要是讲述一个某网站评论爬取的案例 一、单页爬取 目标url:https://ke.qq.com/course/380991/12573838881968191?tuin7265bf35#term_id100454125 import jsonpath import requestsif __name__ __main__:#…

HCIP杂记

动态路由的评判标准 占用资源收敛速度选路 动态路由分类: IGP---内部网关协议 DV型---距离矢量型---RIPLS型---链路状态型---OSPFEGP---外部网关协议 OSPF---无类别的路由协议 组播224.0.0.5和224.0.0.6不存在周期更新机制,仅存在触发更新机制&#xff1…

Elasticsearch如何处理多个关键字查询

Elasticsearch (ES) 是一款强大的全文搜索和分析引擎。当针对Elasticsearch进行含有多个关键字的查询时,ES通常会使用其内置的查询DSL(Domain Specific Language)来处理这些请求。一般来讲,基于多个关键字,可以构建不同…

第十五届蓝桥杯第三期模拟赛第十题 ← 上楼梯

【问题描述】 小蓝要上一个楼梯,楼梯共有 n 级台阶(即小蓝总共要走 n 级)。小蓝每一步可以走 a 级、b 级或 c 级台阶。 请问小蓝总共有多少种方案能正好走到楼梯顶端?【输入格式】 输入的第一行包含一个整数 n 。 第二行包含三个整…

vulfocus环境搭建(kali搭建)

Vulfocus 是一个漏洞集成平台,将漏洞环境 docker 镜像,放入即可使用,开箱即用。 安装docker环境 个人不建议随意更换apt源,我换了几次遇到很多问题。 apt-get update apt-get upgrade(时间很久) apt-get i…

基于springboot的人事管理系统

人事管理系统 摘 要 人事管理系统理工作是一种繁琐的,务求准确迅速的信息检索工作。随着计算机信息技术的飞速发展,人类进入信息时代,社会的竞争越来越激烈,人事就越显示出其不可或缺性,成为学校一个非常重要的模块。…

小米汽车发布了

文章目录 1. 内容回顾2. 发布概要3. 畅想未来 1. 内容回顾 从小米宣布造车开始我就关注了,当时写过相关的文章来分析,这还是三年以前的事情,当时我是不看好小米造车的。 时光飞逝,在三月前,小米发布了它制造的第一台…

防止恶意软件和网络攻击的简单贴士

如今,缺少互联网的生活是难以想象的。然而,互联网的匿名性导致了网络攻击和恶意软件很猖獗。恶意软件会损坏我们的设备、窃取个人数据,并导致金钱损失。因此,保护计算机免受这些威胁显得至关重要。 一、确保操作系统和软件是最新版…

企业数据资产管理的战略价值与实施策略

一、引言 数据资产不仅记录了企业的历史运营情况,更能够揭示市场的未来趋势,为企业的决策提供有力支持。因此,如何有效地管理和利用数据资产,已经成为企业竞争力的重要体现。本文将探讨企业数据资产管理的战略价值与实施策略&…

URL编码:原理、应用与安全性

title: URL编码:原理、应用与安全性 date: 2024/3/29 18:32:42 updated: 2024/3/29 18:32:42 tags: URL编码百分号编码特殊字符处理网络安全应用场景标准演变未来发展 在网络世界中,URL(统一资源定位符)是我们访问网页、发送请求…

设计模式之单例模式精讲

UML图: 静态私有变量(即常量)保存单例对象,防止使用过程中重新赋值,破坏单例。私有化构造方法,防止外部创建新的对象,破坏单例。静态公共getInstance方法,作为唯一获取单例对象的入口…

爬虫第5课:POST请求

下面这段代码是一个简单的Python脚本,用于从Python之禅(一个Python中文技术博客)网站进行搜索查询,并输出搜索结果。以下是对这段代码的详细解释: 导入所需的库: urlopen 和 Request 是从 urllib.request 模…

速通汇编(二)汇编mov、addsub指令

一,mov指令 mov指令的全称是move,从字面上去理解,作用是移动(比较确切的说是复制)数据,mov指令可以有以下几种形式 无论哪种形式,都是把右边的值移动到左边 mov 寄存器,数据&#…

低代码助力企业打造业务管理云平台

随着数字化转型的不断深入,越来越多的企业开始寻求通过构建业务管理云平台来提升运营效率、降低成本并增强市场竞争力。在这个过程中,低代码平台正逐渐成为一种革命性的技术工具,以其高效、灵活的特点助力企业快速搭建和管理云平台。 低代码平…