三问
什么是预积分，为什么用预积分
根据IMU中加速度计和陀螺仪的特性，加速度计测得的加速度和陀螺仪测得的角速度可通过多次积分来获得一段时间内IMU的位姿变化。使用传统积分时，每次优化迭代，估计量都会重新更新，所需的积分计算量很大。
因此，选择预积分来进行处理IMU数据。可以将一段时间内的IMU数据累积，同时还能保证测量值与状态变量无关。
如何用预积分
旋转 R R R 和角度 ω \omega ω 的关系
假设 R R R 是随着时间变化，根据旋转矩阵的性质，有
R T R = I d d t ( R T R ) = R ˙ T R + R T R ˙ = 0 R T R ˙ = − ( R T R ˙ ) T \begin{align} R^TR &= I \\ \frac{d}{dt} (R^TR)&=\dot{R}^TR + R^T \dot{R}=0 \\ R^T\dot{R} &= - (R^T \dot{R})^T \end{align} RTRdtd(RTR)RTR˙=I=R˙TR+RTR˙=0=−(RTR˙)T
上述式子(3)有反对称矩阵的性质，不妨取 ω ∧ = R T R ˙ \omega ^ \wedge = R^T\dot{R} ω∧=RTR˙ ，则有 R ˙ = R ω ∧ (1.1) \dot{R}= R \omega ^\wedge \tag{1.1} R˙=Rω∧(1.1)
在物理意义上，称 ω \omega ω 为瞬时角速度。
式子(1.1)表示为连续时间的形式，求其微分方程的解后离散化为
R ( t ) = R ( t 0 ) exp ( ω ∧ ( t − t 0 ) = R ( t 0 ) E x p ( ω Δ t ) \begin{aligned} R(t) &= R(t_0) ~\exp(\omega^\wedge(t - t_0) \\ &= R(t_0) ~Exp(\omega \Delta t) \end{aligned} R(t)=R(t0) exp(ω∧(t−t0)=R(t0) Exp(ωΔt)
其中 Δ t = t − t 0 \Delta t = t - t_0 Δt=t−t0
坐标系转换
为了简化公式，如 w R w b _wR_{wb} wRwb表示在世界坐标系下的物体的旋转矩阵， 只使用字母 R R R 表示为在世界坐标系下的变量，没有其他角标。
将IMU所在的大地坐标系作为固定的世界坐标系，IMU的测量值角速度 ω ~ \tilde{\omega} ω~ 和 加速度 a ~ \tilde{a} a~ 都是自身坐标系下的，转换到世界坐标系下有
ω ~ = ω a ~ = R T a \begin{aligned} \tilde{\omega} &=\omega \\ \tilde{a} &=R^Ta \end{aligned} ω~a~=ω=RTa
其中 ω \omega ω 和 a a a 为世界坐标系下， R T R^T RT 为 R b w R_{bw} Rbw 表示从世界坐标系到IMU自身坐标系的转换。
在实际过程中，忽略地球自转的干扰，有
ω ~ = ω a ~ = R T ( a − g ) \begin{aligned} \tilde{\omega} &=\omega \\ \tilde{a} &=R^T(a-g) \end{aligned} ω~a~=ω=RT(a−g)
g g g 表示地球的重力，对于 a − g a-g a−g 中的 g g g 定义方式与坐标系的取定有关。通常的坐标系都是Z轴是竖直向上，所以 g = ( 0 , 0 , − 9.8 ) T g=(0, 0, -9.8)^T g=(0,0,−9.8)T，减去 g g g 是和坐标系的取定无关，例如一个IMU静置于桌子上，自身的加速度为0，但在重力的作用下有一个向下的加速度 g g g，因此测量的需要减去 g g g。
![P9420 [蓝桥杯 2023 国 B] 子 2023 / 双子数](https://img-blog.csdnimg.cn/direct/f32e72a965334439974a021af31741f3.png)
