【ORB_SLAM3源码解读】手撕IMU预积分公式

从零开始学习SLAM我学习过写过的代码都在这儿了

代码实战地址：https://github.com/xiaoqiuslamshizhanjiaocheng/xiaoqiuslamshizhanjiaocheng

图文原理地址：https://blog.csdn.net/qq_21950671/category_11508951.html?spm=1001.2014.3001.5482

视频原理地址：https://space.bilibili.com/222855903?spm_id_from=333.1007.0.0

【ORB_SLAM3源码解读】IMU基础介绍、IMU姿态、速度、位置解算以及误差方程、坐标系

惯性系或惯性坐标系相对地面静止的或者做匀速直线运动的参考系，惯性系中牛顿第一、第二定律成立。（牛一：任何物体都要保持匀速直线运动或静止状态，直到外力迫使它改变运动状态为止、牛二：牛顿第二运动定律是力的瞬时作用效果，加速度和力同时产生、同时变化、同时消失。加速度和合力的方向始终保持一致。F=ma）
非惯性系或非惯性坐标系相对地面做加速或者减速非匀速直线运动的参考系，非惯性系中牛顿第一、第二定律不成立。
https://zhuanlan.zhihu.com/p/62150288

牛顿定律

第一定律：假若施加於某物體的外力為零，則該物體的運動速度不變（慣性定律）
第二定律：施加於物體的外力等於此物體的質量與加速度的乘積（加速度定律）
第三定律：當兩個物體相互作用於對方時，彼此施加於對方的力，其大小相等、方向相反（作用力與反作用力定律）

惯性导航的核心原理

位置的导数等于速度
速度的导数等于加速度

理想情况：参考坐标系下载体的初始速度和初始位置已知，利用载体运动过程中参考坐标系下的加速度信息不断地进行积分运算更新实时的速度和位置。

实际情况：加速度计测量的是的载体坐标系下的加速度和角速度，所以需要知道载体坐标系和参考坐标系之间的变换矩阵，将载体坐标系下的加速度和角速度变换到参考坐标系下，最终得到载体在参考坐标系下的加速度和角速度。

而这个参考坐标系就是非惯性系水平地理系，加速度计和陀螺仪的测量数据会受到这个非惯性系水平地理系重力加速度影响。

捷联惯性导航算法：以初始状态为基础，利用 IMU 测量得到的加速度和角速度信息进行积分运算，实时更新载体的位姿及速度等状态。
视觉惯性融合算法：如果知道上一帧图像采样时刻载体的位姿和速度，则可以根据两帧之间的 IMU角速度和加速度积分运算得到当前帧的位姿和速度。将帧与帧之间 IMU 相对测量信息转换为约束节点(载体位姿)的边参与到优化框架中。

李代数

每个三维向量李代数对应一个反对称矩阵(十四讲第二版公式4.11)

三维向量李代数与 SO(3) 旋转矩阵的关系由指数映射(Exponential Map)给定(十四讲第二版公式4.14)

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’sFormula )表明

六维向量(前三维为平移后三维为旋转)的李代数与SE(3)变换矩阵的关系由指数映射给定

BCH 公式与近似形式求旋转矩阵的扰动也就是旋转矩阵的增量

SO(3) 李代数上的求导

构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。

用李代数表示姿态，然后根据李代数加法来对李代数求导。

对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

陀螺测量模型

b g 是随时间缓慢变化的 bias, η g 是白噪声

加速度计测量模型

aw 是加速度计的加速度在水平地理系下的值，静止时加速度计测量到与重力方向相反的值；
gw 是水平地理系下重力加速度的值， aw - gw 就是除重力外实际的加速度计的加速度，

运动模型的微分方程

运动方程的离散形式

欧拉积分

中值积分

一段时间内的积分

角度＝角速度乘时间
v = v0 + at
x = v0t + 1/2at*t

角速度乘时间等于角度，角度要去掉噪声和初始值，角度通过指数映射可以求旋转矩阵，旋转矩阵连乘，理解为求和，最后乘法ｉ时刻的旋转矩阵，最后是ｊ时刻的旋转矩阵．

速度和位置要除去重力的影响，还要从载体坐标系转换到大地坐标系．

预积分

微分

积分

一阶导数是对原函数求导

二阶导数是对一阶导数求导

混合偏导数就是二阶导数，先对x求导数，再次对y求导数

圆的曲率是K，半径是R，半径越小的圆曲率越大，直线可以看作半径为无穷大的圆

雅可比矩阵

在向量分析中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵, 其行列式称为雅可比行列式。

海森矩阵

海森矩阵(Hessian matrix)是一个自变量为向量的实值函数的二阶偏导数组成的方块矩阵（假设其二阶偏导都存在）。
海森矩阵是一个多元函数的二阶偏导数构成的方阵可以用矩阵的正定性来确定极值。

隐含数

如果方程F(x,y)=0能确定y是x的函数，那么称这种方式表示的函数是隐函数。

隐含数求导法则

隐含数 (x²)+(y²)-(r²)=0，即2x+2yy’=0，于是得隐含数导数 y ’ =-x/y 。

方向导数是各个方向上的导数

偏导数连续才有梯度存在

梯度的方向是方向导数中取到最大值的方向，梯度的值是方向导数的最大值

导数

是指函数在某一点处变化的快慢,是一种变化率。

微分

是指函数在某一点处（趋近于无穷小）的变化量，是一种变化的量。
https://www.zhihu.com/question/22199657/answer/115178055

高等数学之函数，极限，导数，微分，积分
这五个词，我认为是高等数学的基础，一切都是他们的延伸，尤其是后三个用到的最多。
通过我对书的理解和思考，我对这几个词的概念有了总结。
首先，函数是两组数之间的对应关系的规律的体现。是一点的一个数到另一数的对应的规则，也既x到y。

极限

则是任意一点到一个固定点之间的关系，当两点无限接近时，总能体现出函数所对应的关系，并且存在一个固定数值。极限的对象是y。

导数

是两个任意点无限接近时变化趋势的体现。这个变化趋势是用两个增量的比值体现的，也可以说是斜率，也可以说是y将要因x改变多少自己要改变他的多少倍。导数将函数体现的的规律细分。

微分

就是将导数的那个关键：导数=△y/△x做了更细致的定义，△x*导数叫微分。

积分

是通过导数找到原函数。
总体来说，导数的理解才是关键，它起到了承前启后的作用。
https://www.douban.com/note/604891469/

求微分其实就是线性化，导数其实就是线性空间之间的线性变换，Jaocibian矩阵本质上就是导数。

极限

：微积分的基础是极限，因为微分用到无穷小的概念，所以有了数列和极限的严格数学定义。

导数

：导数是割线的极限，是斜率，是以直代曲，是微分，微分是为了积分。所以导数是积分的工具，导数可以用来求积分。
导数 和 极限 是微分的工具 微分是为了积分 积分就是求面积 求体积 求路程
微分和导数其实是一个概念，有了导数自然可以求出微分，而微分存在必然有导数，

微分

：以直代曲，无限分下去，曲线下微小的矩形是“微分”，微分是线性映射，微分是对曲线的近似。

积分

：求和，把这些“微分”加起来就是“积分”，就可以得到曲线下的面积，是微分的无限求和

微积分

：微分到积分的过程称为“微积分”

感觉我们一直都被骗了 别表面现象骗了 被细节骗了 因为要解决积分问题也就是求面积的问题，自然就有微分进而有了极限，因为极限又划分为好多种所以就有后面的函数极限的一系列研究，而这些都是为了计算，所以我们要抓住的关键就是积分，其他都是细枝末节，为这个服务的，而这个就是所谓的重点。

认知中线性计算由于非线性计算采用各种方法讲非线性转化为线性，比如坐标变换 然后用善于做线性运算的线性代数去计算， 单变量多变量等什么计算啊名词啊都是吓唬人的你不是数学家 不是专门搞计算的原理知道就好了至于计算就交给计算机吧！生命短暂。

再一次强调 切线就是导数 导数就是切线

极限的计算方法可以帮助我们计算积分也就是面积。

把矩形面积加起来就需要计算级数（级数就是数列的和），但级数的计算往往很复杂。
不管是为了计算更加简便，还是为了扩大微积分的应用范围，都需要把“微分”这个概念抽象出来。
为了更好的计算积， 就要寻找更好的“微”方法。
差分是的求和等于总和，微分的求和近似等于总和，差分不好计算，微分是为了计算

斜率的极限是割线，割线被定义为导数

这个公式解释了导数计算的原理

原理求导也有很多规则 所一最终还是求导数。
导数微积分服务 最终还是要计算
所以关于导数 和极限的计算 有涉及很多的内容
总之 你先要把握全局 不要陷入细节 所谓 先粗后精细

这里也体现了无限的差分和微分近似

多边形是直线组成的，圆是曲线，所以这种思想叫做线性近似或者以直代曲。

“现代分析”是“数学严格化”的意思