Vins-mono中的IMU预积分【SLAM】

世界系下连续时间的IMU积分

w w w代表世界系, b k b_{k} bk代表第k帧图像。
[ t k , t k + 1 ] [t_{k}, t_{k+1}] [tk,tk+1]时间段内,有通过加速度和角速度在连续时间下的积分:
p b k + 1 w = p b k w + v b k w Δ t k + ∬ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 p^{w}_{b_{k+1}} = p^{w}_{b_{k}} + v^{w}_{b_{k}} \Delta t_{k} + \iint_{t \in [t_{k}, t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{a_{t}}-n_{a})-g^{w})dt^{2} pbk+1w=pbkw+vbkwΔtk+t[tk,tk+1](Rtw(a^tbatna)gw)dt2
v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t v^{w}_{b_{k+1}} = v^{w}_{b_{k}} + \int_{t \in [t_{k}, t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{a_{t}}-n_{a})-g^{w})dt vbk+1w=vbkw+t[tk,tk+1](Rtw(a^tbatna)gw)dt
q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( ω ^ t − b ω t − n ω ) q t b k d t q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{\omega}_{t}-b_{\omega_{t}}-n_{\omega})q^{b_{k}}_{t} dt qbk+1w=qbkwt[tk,tk+1]21Ω(ω^tbωtnω)qtbkdt

其中 Δ t k = t k + 1 − t k \Delta t_{k} = t_{k+1} - t_{k} Δtk=tk+1tk, Ω ( ⋅ ) \Omega (\cdot) Ω()是四元数乘法右乘矩阵的虚数部分, Ω ( ω ) = [ − [ ω ] × ω − ω T 0 ] \Omega(\omega) = \begin{bmatrix} -[\omega]_{\times} & \omega \\ -\omega^{T} & 0 \end{bmatrix} Ω(ω)=[[ω]×ωTω0]

IMU预积分的形成

对上一部分的三个积分公式转换坐标系就可以形成预积分的定义式:
R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R^{b_{k}}_{w}p^{w}_{b_{k+1}} = R^{b_{k}}_{w}(p^{w}_{b_{k}} + v^{w}_{b_{k}} \Delta t_{k} - \frac{1}{2} g^{w} \Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}} Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk21gwΔtk2)+αbk+1bk
R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k R^{b_{k}}_{w}v^{w}_{b_{k+1}} = R^{b_{k}}_{w}(v^{w}_{b_{k}} - g^{w} \Delta t_{k}) + \beta^{b_{k}}_{b_{k+1}} Rwbkvbk+1w=Rwbk(vbkwgwΔtk)+βbk+1bk
q w b k ⊗ q b k + 1 w = γ b k + 1 b k q^{b_{k}}_{w} \otimes q^{w}_{b_{k+1}} = \gamma^{b_{k}}_{b_{k+1}} qwbkqbk+1w=γbk+1bk
其中 α 、 β 、 γ \alpha、\beta、\gamma αβγ就是IMU预积分量。
α b k + 1 b k = ∬ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 \alpha^{b_{k}}_{b_{k+1}} = \iint_{t \in [t_{k}, t_{k+1}]} R^{b_{k}}_{t}(\hat{a}_{t}-b_{a_{t}}-n_{a})dt^{2} αbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt2
β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t \beta^{b_{k}}_{b_{k+1}} = \int_{t \in [t_{k}, t_{k+1}]} R^{b_{k}}_{t}(\hat{a}_{t}-b_{a_{t}}-n_{a})dt βbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt
γ b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( ω ^ t − b ω t − n ω ) γ t b k d t \gamma^{b_{k}}_{b_{k+1}} = \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{\omega}_{t}-b_{\omega_{t}}-n_{\omega})\gamma^{b_{k}}_{t} dt γbk+1bk=t[tk,tk+1]21Ω(ω^tbωtnω)γtbkdt
可以看出预积分量只和状态量中的零偏有关,如果零偏更新了,就用预积分对零偏的一阶雅可比更新预积分。

离散时间下的IMU预积分递推公式

实际IMU信息是离散的,所以要得到离散时间下的IMU预积分公式,可以用Euler、中值积分、RK4进行数值积分求IMU预积分。
离散时间下IMU预积分量的递推公式为:
α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t 2 \hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}^{b_{k}}_{i} \delta t + \frac{1}{2} R(\hat{\gamma}^{b_{k}}_{i})(\hat{a}_{i} - b_{a_{i}}) \delta t^{2} α^i+1bk=α^ibk+β^ibkδt+21R(γ^ibk)(a^ibai)δt2
β ^ i + 1 b k = β ^ i b k + R ( γ ^ i b k ) ( a ^ i − b a i ) δ t \hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + R(\hat{\gamma}^{b_{k}}_{i})(\hat{a}_{i} - b_{a_{i}}) \delta t β^i+1bk=β^ibk+R(γ^ibk)(a^ibai)δt
γ ^ i + 1 b k = γ ^ i b k ⊗ [ 1 1 2 ( ω ^ i − b ω i ) δ t ] \hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}(\hat{\omega}_{i} - b_{\omega_{i}})\delta t \end{bmatrix} γ^i+1bk=γ^ibk[121(ω^ibωi)δt]
其中 i i i表示第 i i i帧imu信息。
如果用中值积分方法,预积分更新顺序为 γ 、 α 、 β \gamma、\alpha、\beta γαβ

误差卡尔曼(ESKF)求预积分量的协方差矩阵

误差卡尔曼中状态量为包含 α 、 β 、 θ 、 b a t 、 b ω t \alpha、\beta、\theta、b_{{a}_{t}}、b_{{\omega}_{t}} αβθbatbωt共15维的状态变量。其中旋转姿态用 θ \theta θ而不再用 γ \gamma γ表示,这样做的原因是四元数表示旋转本身是过参数化的,另外用旋转向量表示求雅可比矩阵方便一些,符合平常的计算习惯,平常不用旋转向量的原因是旋转向量具有周期性,但是在误差卡尔曼中不存在周期性这个问题,误差都是小量。

于是按照误差卡尔曼的推导方式可以得到连续时间下误差状态量的导数公式:
δ x ˙ t b k = [ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ ω t ] = [ 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] × − R t b k 0 0 0 − [ ω ^ − b ω t ] × 0 − I 0 0 0 0 0 0 0 0 0 0 ] [ δ α t b k δ β t b k δ θ t b k δ b a t δ b ω t ] + [ 0 0 0 0 − R t b k 0 0 0 0 − I 0 0 0 0 I 0 0 0 0 I ] [ n a n ω n b a n b ω ] = F t δ x t b k + G t n t \delta \dot{x}^{b_{k}}_{t} = \begin{bmatrix} \delta \dot{\alpha}^{b_{k}}_{t} \\ \delta \dot{\beta}^{b_{k}}_{t} \\ \delta \dot{\theta}^{b_{k}}_{t} \\ \delta \dot{b}_{a_{t}} \\ \delta \dot{b}_{\omega_{t}} \end{bmatrix} = \begin{bmatrix} 0 & I & 0 & 0 & 0\\ 0 & 0 & -R^{b_{k}}_{t}[\hat{a}_{t} - b_{a_{t}}]_{\times} & -R^{b_{k}}_{t} & 0\\ 0 & 0 & -[\hat{\omega} - b_{\omega_{t}}]_{\times} & 0 & -I\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta \alpha^{b_{k}}_{t} \\ \delta \beta^{b_{k}}_{t} \\ \delta \theta^{b_{k}}_{t} \\ \delta b_{a_{t}} \\ \delta b_{\omega_{t}} \end{bmatrix} + \begin{bmatrix} 0 & 0 & 0 & 0\\ -R^{b_{k}}_{t} & 0 & 0 & 0\\ 0 & -I & 0 & 0\\ 0 & 0 & I & 0\\ 0 & 0 & 0 & I \end{bmatrix} \begin{bmatrix} n_{a} \\ n_{\omega} \\ n_{b_{a}} \\ n_{b_{\omega}} \end{bmatrix} = F_{t}\delta x^{b_{k}}_{t} + G_{t} n_{t} δx˙tbk= δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙ωt = 00000I00000Rtbk[a^tbat]×[ω^bωt]×000Rtbk00000I00 δαtbkδβtbkδθtbkδbatδbωt + 0Rtbk00000I00000I00000I nanωnbanbω =Ftδxtbk+Gtnt
通过这个连续时间的导数公式,可以推出离散时间下的误差状态变量的递推公式
δ x i + 1 b k = δ x i b k + ( F t δ x i b k + G t n t ) δ t = ( I + F t δ t ) δ x k + G t δ t ⋅ n t \delta x^{b_{k}}_{i+1} = \delta x^{b_{k}}_{i} + (F_{t} \delta x^{b_{k}}_{i} + G_{t}n_{t})\delta t = (I+F_{t} \delta t)\delta x_{k} + G_{t} \delta t \cdot n_{t} δxi+1bk=δxibk+(Ftδxibk+Gtnt)δt=(I+Ftδt)δxk+Gtδtnt
注意!这里离散化用的是中值积分,所以 n t n_{t} nt变成了18维的向量,包含了 n a i 、 n ω i 、 n a i + 1 、 n ω i + 1 、 n b a 、 n b ω n_{a_{i}}、n_{\omega_{i}}、n_{a_{i+1}}、n_{\omega_{i+1}}、n_{b_{a}}、n_{b_{\omega}} nainωinai+1nωi+1nbanbω
最终结果太复杂了,直接截图,下面图片是cvlife课程的资料:
在这里插入图片描述
图片中和本篇博客差别在于图片中的 k k k表示第 k k k帧imu信息,本篇博客为了区别图像帧和imu帧用了 i i i表示第 i i i帧imu信息。
为了方便说明,将这个大公式写作:
δ x i + 1 = F δ x i + V n \delta x_{i+1} = F \delta x_{i} + V n δxi+1=Fδxi+Vn

重要! 误差状态量和实际状态量雅可比矩阵的关系

我们上面推出的是误差状态量 δ x = [ δ α δ β δ θ δ b a δ b ω ] \delta x = \begin{bmatrix} \delta \alpha \\ \delta \beta \\ \delta \theta \\ \delta b_{a} \\ \delta b_{\omega} \end{bmatrix} δx= δαδβδθδbaδbω 的递推公式,实际上我们也比较关心状态量 x = [ α β θ b a b ω ] x = \begin{bmatrix} \alpha \\ \beta \\ \theta \\ b_{a} \\ b_{\omega} \end{bmatrix} x= αβθbabω 对状态量 x x x的雅可比矩阵,其实二者有很大的关系。

已知状态量的递推关系:
x i + 1 t = f ( x i t ) x_{i+1}^{t} = f(x_{i}^{t}) xi+1t=f(xit)
这里上标 t t t表示实际状态变量,区分于名义状态变量。
按照误差卡尔曼的方式写成名义状态变量和误差状态变量,有:
x i + 1 + δ x i + 1 = f ( x i + δ x i ) x_{i+1} + \delta x_{i+1} = f(x_{i} + \delta x_{i}) xi+1+δxi+1=f(xi+δxi)

然后把 f ( ⋅ ) f(\cdot) f()一阶泰勒展开:
x i + 1 + δ x i + 1 = f ( x i ) + J δ x i x_{i+1} + \delta x_{i+1} = f(x_{i}) + J\delta x_{i} xi+1+δxi+1=f(xi)+Jδxi
其中, J = ∂ x i + 1 t ∂ x i J = \frac{\partial x_{i+1}^{t}}{\partial x_{i}} J=xixi+1t

又因为名义状态变量满足:
x i + 1 = f ( x i ) x_{i+1} = f(x_{i}) xi+1=f(xi)
所以有:
δ x i + 1 = J δ x i \delta x_{i+1} = J \delta x_{i} δxi+1=Jδxi
也就是说,上一部分中误差状态变量递推公式的大F矩阵就是这里实际状态变量对i时刻名义状态变量的雅可比矩阵,即:
F = J = ∂ x i + 1 t ∂ x i F = J = \frac{\partial x_{i+1}^{t}}{\partial x_{i}} F=J=xixi+1t

同时这里还有一点, x i + 1 t = x i + 1 + δ x i + 1 x_{i+1}^{t} = x_{i+1} + \delta x_{i+1} xi+1t=xi+1+δxi+1对x_{i}求偏导,其中 x i + 1 = f ( x i ) x_{i+1} = f(x_{i}) xi+1=f(xi),而 δ x i + 1 \delta x_{i + 1} δxi+1 x i x_{i} xi无关,所以 F = J = ∂ x i + 1 ∂ x i F=J= \frac{\partial x_{i+1}}{\partial x_{i}} F=J=xixi+1

然后考虑如果要求 i + 1 i+1 i+1时刻实际状态变量对初始时刻的雅可比矩阵,不妨设 J i = ∂ x i ∂ x J_{i} = \frac{\partial x_{i}}{\partial x} Ji=xxi x x x代表本次预积分初始时刻的名义状态变量,然后有:
J i + 1 = F J i J_{i+1} = F J_{i} Ji+1=FJi

维护这个雅可比矩阵的主要作用是,其中包含了实际预积分量对零偏的雅可比矩阵,当零偏更新时,可以利用这个雅可比矩阵更新预积分量。

δ x i + 1 = F δ x i + V n \delta x_{i+1} = F \delta x_{i} + V n δxi+1=Fδxi+Vn可知误差状态变量的协方差矩阵更新方式:
P i + 1 = F P i F T + V Q V T P_{i+1} = FP_{i}F^{T} + VQV^{T} Pi+1=FPiFT+VQVT
其中, P i P_{i} Pi δ x i \delta x_{i} δxi的协方差矩阵, Q Q Q n n n的协方差矩阵。
由于实际状态变量和误差状态变量的关系为:
x i t = x i + δ x i x_{i}^{t} = x_{i} + \delta x_{i} xit=xi+δxi
其中名义状态变量协方差矩阵为0,所以实际状态变量的协方差矩阵等于误差状态变量的协方差矩阵。

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

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

相关文章

插入排序的可视化实现(Python)

插入排序的Python代码 import tkinter as tk import random import timeclass InsertionSortVisualizer:def __init__(self, root, canvas_width800, canvas_height400, num_bars10):self.root rootself.canvas_width canvas_widthself.canvas_height canvas_heightself.nu…

单片机 VS 嵌入式LInux

linux 嵌入式开发岗位需要掌握Linux的主要原因之一是,许多嵌入式系统正在向更复杂、更功能丰富的方向发展,需要更强大的操作系统支持。而Linux作为开源、稳定且灵活的操作系统,已经成为许多嵌入式系统的首选。以下是为什么嵌入式开发岗位通常…

国家市场监督管理总局令第90号

非法定计量单位限制使用管理办法 2024年3月18日国家市场监督管理总局令第90号公布 自2024年6月1日起施行 第一条 为了保证国家计量单位制的统一,加强非法定计量单位的使用管理,根据《中华人民共和国计量法》等有关法律、行政法规,制定本办…

什么是掩码补丁位置?

掩码补丁位置(Masked Patch Position)是一个在视觉任务处理中使用的概念,特别是在处理图像分割等任务时。 在深度学习和计算机视觉领域,掩码(Mask)常用于指示模型关注或忽略输入数据的特定部分。在基于补丁…

登录的几种方式

一、session 1、客户端发送请求,服务器将登录信息存储在 Session 中,Session 依赖于 Cookie(cookie指的就是在浏览器里面存储的一种数据,仅仅是浏览器实现的一种数据存储功能。Cookie实际上是一小段的文本信息。)&…

【行为型模式】状态模式

一、状态模式概述 状态模式的定义:允许对象在内部状态改变时改变它的行为,对象看起来好像修改了它的类。(对象行为型) 策略模式和状态模式是双胞胎,在出生时才分开。 策略模式是围绕可以互换的算法来创建成功业务的。状态模式走的是更崇高的路&#xff0…

全民拼购:社交电商与拼购玩法的完美结合

大家好,我是微三云周丽,今天给大家分析当下市场比较火爆的商业模式! 小编今天跟大伙们分享什么是社交电商与拼购玩法的wan美结合? 随着社交电商和拼购玩法的兴起,全民拼购模式正逐渐成为电商行业的新宠。结合了人性化…

CSS中的display: flex;

display: flex; 是CSS(层叠样式表)中用于设置容器为弹性盒子模型(Flexbox)的属性值。弹性盒子模型是CSS3中引入的一个新的布局模式,它设计用来更好地在容器中布局、对齐和分配空间给子元素(即容器内的项目&…

Github账号注册

天行健,君子以自强不息;地势坤,君子以厚德载物。 每个人都有惰性,但不断学习是好好生活的根本,共勉! 文章均为学习整理笔记,分享记录为主,如有错误请指正,共同学习进步。…

深度学习与神经网络入门

前言 人工智能(AI)与机器学习(ML)与深度学习(DL)的关系: DL包含于ML,ML包含于AI。 即深度学习是机器学习一部分,机器学习又是人工智能的一个分支。 那么深度学习到底有…

运气变好的几个小妙招

首先不要总是说一些丧气话,因为老天爷是听不到主语的,即使你当时说的是”谁谁谁好烦人啊“类似的句式,但传到上帝耳朵的时候会失去主语,也就会潜移默化的带到自己身上。 那我们应该怎么做呐? 1. 避免消极语言 首先&…

本文为解决:前端json传String格式时间参数,后端无法解析。

有的人说单独写一个localDateTime配置类,看了一下可以不用。 直接写WebMvcConfig里面就行 import com.fasterxml.jackson.databind.ObjectMapper; import com.fasterxml.jackson.databind.module.SimpleModule; import com.fasterxml.jackson.databind.ser.std.ToS…

CSS flex弹性布局属性

容器属性 flex-direction 设置主轴方向,默认水平方向 row 默认值,主轴为水平方向(从左向右排水平布局) row-reverse 主轴为水平方向(从右向左排水平布局) column 主轴为垂直方向(从上到下垂直布局&#x…

学习笔记——微信小程序var与let、bindtap与bindinput、全局变量与局部变量的区别

1、var与let的区别 var申明的为全局变量,作用域为所在的函数内,其他函数调用会出现变量未定义的报错 let声明的为局部变量,只会作用于最近的{}中,其他区域无法调用 2、全局变量与局部变量 全局变量是在整个小程序运行周期内都…

openGauss学习笔记-268 openGauss性能调优-TPCC性能调优测试指导-网络配置-网卡多中断队列设置

文章目录 openGauss学习笔记-268 openGauss性能调优-TPCC性能调优测试指导-网络配置-网卡多中断队列设置268.1 中断调优268.2 网卡固件确认与更新 openGauss学习笔记-268 openGauss性能调优-TPCC性能调优测试指导-网络配置-网卡多中断队列设置 本章节主要介绍openGauss数据库内…

B203-若依框架应用

目录 简介版本RuoYi-fast项目准备新增模块/代码生成 简介 基于SpringBoot的权限管理系统,基于SpringBoot开发的轻量级Java快速开发框架 版本 前后端未分离单应用版本:RuoYi-fast,前后端未分离多模块版本:RuoYi 前后端分离单应用…

转行做银行测试,需要了解哪些?

在这个内卷严重的时代,银行的业务不断增加,随着软件信息化的要求越来越高,银行对软件测试人员也提出了非常高的要求。 银行的软件测试是针对银行的软件系统(如柜面系统、信贷系统)和银行专用设备(如ATM机、…

浅谈薪酬绩效设计及运行的忌讳

薪酬绩效设计及运行是企业管理中的重要环节,直接关系到员工的工作积极性和企业的整体效益。然而,在实际操作中,许多企业往往因为对薪酬绩效设计的不当理解或操作不当,导致了一系列问题的出现。本文将从薪酬绩效设计及运行的忌讳入…

一键还原精灵 V12.1.405.701 装机版

网盘下载 个人版:不划分分区不修改分区表及MBR,安装非常安全,备份文件自动隐藏,不适用于WIN98系统。 装机版:需用PQMAGIC划分分区作隐藏的备份分区,安装过程中有一定的风险,安装后就非常安全。…

基于Spring Cloud Alibaba的微服务业务拆分设计

胡弦,视频号2023年度优秀创作者,互联网大厂P8技术专家,Spring Cloud Alibaba微服务架构实战派(上下册)和RocketMQ消息中间件实战派(上下册)的作者,资深架构师,技术负责人,极客时间训练营讲师,四…