IMU预积分

IMU预积分

referrence：

VINS-MONO解析——IMU预积分https://blog.csdn.net/iwanderu/article/details/104623177

自动驾驶与机器人中的SLAM从技术理论与实践

为什么要积分

IMU获取的是加速度和角速度，通过对IMU测量量的积分操作，能够获得机器人的位姿信息。

IMU测量值包括两方面

• 加速度计得到的线加速度 ${\hat{a}}_t=R_w^t(a_t-g^w)+b_{at}+n_a$

• 陀螺仪得到的角速度。$\widehat{w_t}=w_t+b_{wt}+n_w$

上标表示测量值，没有上标为真实值

• 加速度偏置$b_{at}$
• 角速度偏置$w_{at}$
• 加速度噪声$n_a$
• 角速度噪声$n_w$

偏置是由IMU内部的机电测量装置导致的，同时受温度等因素的影响，随时间发生变化

噪声服从高斯分布$n_a\sim N(0,{\sigma }_a^2，n_w\sim N(0,{\sigma }_w^2)$

$R_w^t$是从世界坐标系到IMU坐标系的旋转矩阵

对于图像帧k和k+1，体坐标系对应为bk和bk+1，每周期PVQ（位置、速度和姿态）可以根据[tk,tk+1]时间间隔内的IMU测量值，在世界坐标系下进行传递。

因此IMU预积分的立足点就落在相邻两帧（点云）之间的IMU积分测量值计算及其动态更新上，获得每周期PVQ（位置、速度和姿态）增量的测量值，对照其他通过非IMU方式获得的PVQ增量的估计值，进而获得PVQ增量的残差。然后以该残差构造代价函数对每个节点的PVQ进行迭代更新和优化。

预积分定义

从IMU系统的运动学模型出发，考虑其中的五个变量

旋转R，平移p，角速度$w$，线速度$v$与加速度$a$，运动学关系可以写成：
$$\begin{gathered} \dot{R}=R{w}^{\wedge }, \\ \dot{p}=v, \\ \dot{v}=a \end{gathered}$$
$由t到\Delta t$时间内的积分，可得：
$$\begin{gathered} R(t+\Delta t)=R(t)Exp(w(t)\Delta t), \\ v(t+\Delta t)=v(t)+a(t)\Delta t, \\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}a(t)\Delta t^2 \end{gathered}$$
这里的Exp为指数映射，也是罗德里格斯公式，依据高翔书中的定义，$Exp(w)=exp(w^{\wedge })$
对于角速度和加速度

• 线加速度 ${\hat{a}}_t(t)=R_w^t(a_t(t)-g^w)+b_{at}(t)+n_a(t)$

• 角速度。$\widehat{w_t}(t)=w_t(t)+b_{wt}(t)+n_w(t)$

代入上式为直接积分，与EKF中的预测过程并无区别。

直接积分的缺点是，它描述的过程和状态量有关。如果对i时刻的状态进行优化，i+1,i+2,…,j-1时刻的状态也会跟着改变，则需要重新计算积分。因此引出了预积分，将IMU读数放在一侧，状态量放在另一侧，于是，定义的相对运动量为：
$$\begin{gathered} \Delta R_{ij}=R_i^T{R}_j=\prod _{k=i}^{j-1}Exp(({\tilde{w}}_k-b_{g,k}-n_{gd,k})\Delta t), \\ \Delta v_{ij}=R_i^T(v_j-v_i-g\Delta t_{ij})=\sum _{k=i}^{j-1}\Delta R_{ik}({\tilde{a}}_k-b_{a,k}-n_{ad,k})\Delta t, \\ \Delta p_{ij}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2) \\ =\sum _{k=i}^{j-1}[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}({\tilde{a}}_k-b_{a,k}-n_{ad,k})\Delta t^2] \end{gathered}$$
IMU的采样频率是高于图像帧的发布频率的，所以相邻两个图像帧之间的IMU信息需要积分从而与视觉信息对齐。

预积分的主要过程：

1. 在上一时刻数据的基础上，计算预积分观测量：$\Delta \widetilde{R_{ij}},\Delta \widetilde{v_{ij}},\Delta \widetilde{p_{ij}}$
2. 计算三个噪声量的协方差矩阵，作为图优化的信息矩阵。
3. 计算预积分观测量相对于零偏(偏置)的雅可比矩阵，共五个R,p,w,v,a