IMU预积分
IMU预积分
referrence：
VINS-MONO解析——IMU预积分https://blog.csdn.net/iwanderu/article/details/104623177
自动驾驶与机器人中的SLAM从技术理论与实践
为什么要积分
IMU获取的是加速度和角速度，通过对IMU测量量的积分操作，能够获得机器人的位姿信息。
IMU测量值包括两方面
-
加速度计得到的线加速度 a ^ t = R w t ( a t − g w ) + b a t + n a \hat{a}_t=R_w^t(a_t-g^w)+b_{at}+n_a a^t=Rwt(at−gw)+bat+na
-
陀螺仪得到的角速度。 w t ^ = w t + b w t + n w \hat{w_t}=w_t+b_{wt}+n_w wt^=wt+bwt+nw
上标表示测量值，没有上标为真实值
- 加速度偏置 b a t b_{at} bat
- 角速度偏置 w a t w_{at} wat
- 加速度噪声 n a n_a na
- 角速度噪声 n w n_w nw
偏置是由IMU内部的机电测量装置导致的，同时受温度等因素的影响，随时间发生变化
噪声服从高斯分布 n a ∼ N ( 0 , σ a 2 ， n w ∼ N ( 0 , σ w 2 ) n_a\sim{N}(0,\sigma^2_a，n_w\sim{N}(0,\sigma^2_w) na∼N(0,σa2，nw∼N(0,σw2)
R w t R^t_w Rwt是从世界坐标系到IMU坐标系的旋转矩阵
对于图像帧k和k+1，体坐标系对应为bk和bk+1，每周期PVQ（位置、速度和姿态）可以根据[tk,tk+1]时间间隔内的IMU测量值，在世界坐标系下进行传递。
因此IMU预积分的立足点就落在相邻两帧（点云）之间的IMU积分测量值计算及其动态更新上，获得每周期PVQ（位置、速度和姿态）增量的测量值，对照其他通过非IMU方式获得的PVQ增量的估计值，进而获得PVQ增量的残差。然后以该残差构造代价函数对每个节点的PVQ进行迭代更新和优化。
预积分定义
从IMU系统的运动学模型出发，考虑其中的五个变量
旋转R，平移p，角速度 w w w，线速度 v v v与加速度 a a a，运动学关系可以写成：
R ˙ = R w ∧ , p ˙ = v , v ˙ = a \dot{R}=Rw^\wedge ,\\ \dot{p}=v,\\ \dot{v}=a R˙=Rw∧,p˙=v,v˙=a
由 t 到 Δ t 由t到\Delta t 由t到Δt时间内的积分，可得：
R ( t + Δ t ) = R ( t ) E x p ( w ( t ) Δ t ) , v ( t + Δ t ) = v ( t ) + a ( t ) Δ t , p ( t + Δ t ) = p ( t ) + v ( t ) Δ t + 1 2 a ( t ) Δ t 2 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 R(t+Δt)=R(t)Exp(w(t)Δt),v(t+Δt)=v(t)+a(t)Δt,p(t+Δt)=p(t)+v(t)Δt+21a(t)Δt2
这里的Exp为指数映射，也是罗德里格斯公式，依据高翔书中的定义， E x p ( w ) = e x p ( w ∧ ) Exp(w)=exp(w^\wedge) Exp(w)=exp(w∧)
对于角速度和加速度
-
线加速度 a ^ t ( t ) = R w t ( a t ( t ) − g w ) + b a t ( t ) + n a ( t ) \hat{a}_t(t)=R_w^t(a_t(t)-g^w)+b_{at}(t)+n_a(t) a^t(t)=Rwt(at(t)−gw)+bat(t)+na(t)
-
角速度。 w t ^ ( t ) = w t ( t ) + b w t ( t ) + n w ( t ) \hat{w_t}(t)=w_t(t)+b_{wt}(t)+n_w(t) wt^(t)=wt(t)+bwt(t)+nw(t)
代入上式为直接积分，与EKF中的预测过程并无区别。
直接积分的缺点是，它描述的过程和状态量有关。如果对i时刻的状态进行优化，i+1,i+2,…,j-1时刻的状态也会跟着改变，则需要重新计算积分。因此引出了预积分，将IMU读数放在一侧，状态量放在另一侧，于是，定义的相对运动量为：
Δ R i j = R i T R j = ∏ k = i j − 1 E x p ( ( w ~ k − b g , k − n g d , 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 − n a d , 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 − n a d , k ) Δ t 2 ] \Delta R_{ij}=R_i^TR_j=\prod_{k=i}^{j-1}Exp((\tilde w_k-b_{g,k}-n_{gd,k})\Delta t),\\ \Delta v_{ij}=R^T_i(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^T_i(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t^2_{ij})\\ =\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] ΔRij=RiTRj=k=i∏j−1Exp((w~k−bg,k−ngd,k)Δt),Δvij=RiT(vj−vi−gΔtij)=k=i∑j−1ΔRik(a~k−ba,k−nad,k)Δt,Δpij=RiT(pj−pi−viΔtij−21gΔtij2)=k=i∑j−1[ΔvikΔt+21ΔRik(a~k−ba,k−nad,k)Δt2]
IMU的采样频率是高于图像帧的发布频率的，所以相邻两个图像帧之间的IMU信息需要积分从而与视觉信息对齐。
预积分的主要过程：
- 在上一时刻数据的基础上，计算预积分观测量： Δ R i j ~ , Δ v i j ~ , Δ p i j ~ \Delta \tilde{R_{ij}},\Delta \tilde{v_{ij}},\Delta \tilde{p_{ij}} ΔRij~,Δvij~,Δpij~
- 计算三个噪声量的协方差矩阵，作为图优化的信息矩阵。
- 计算预积分观测量相对于零偏(偏置)的雅可比矩阵，共五个R,p,w,v,a