1 背景
在一般的车载传感器上，都会提供IMU惯性测量单元，它主要提供各个时刻的线性加速度(沿着x、y、z的加速度)以及角速度(绕着x、y、z方向的角速度)测量值。由于这些测量值之间与车本身的位姿有一定关系，因此可以通过IMU测量值来估计车本身的位姿。
2 原理
学过初中物理就应该知道，一个物体，已知速度与加速度，这时经过一段时间后，可以计算出各个时间点的速度以及位移值，在整个过程计算中速度、加速度在不断变化，因此求每个时刻的值需要用到积分。由于处理的离散数据，这里的积分可直接累计（累加）的方式进行积分近似。因此所有公式均是基于此推导出来。唯一不同的是，公式里会涉及到旋转矩阵。这里根据文献[1]，提供如下的IMU的数学模型
(1)
$$
\begin{aligned}
\bar{\omega}_b(t) &= \omega_b(t) + b^\omega(t)+\eta^\omega(t)
\\
\bar{a}_b(t)&=R_{wb}^T(a_w(t)-g_w(t)) + b^a(t)+\eta^a(t)
\end{aligned}
$$
公式中参数含义如下：
\(\bar{\omega}_b(t)\):表示\(t\)时刻测量到车载坐标系下的角速度。
\(\omega_b(t)\):表示\(t\)时刻计算到的车载坐标系下的角速度。
\(b^\omega(t)\):表示\(t\)时刻角速度的偏置，这个参数需要优化。
\(\eta^\omega(t)\):表示\(t\)时刻角速度的白噪声。
\(\bar{a}_b(t)\):表示\(t\)时刻测量到车载坐标系下的线性加速度。
\(a_w(t)\):表示\(t\)时刻计算到的世界坐标系下的线性加速度。
\(g_w(t)\):表示\(t\)时刻世界坐标系下的重力加速度。
\(b^a(t)\):表示\(t\)时刻线性加速度的偏置，这个参数需要优化。
\(\eta^a(t)\):表示\(t\)时刻线性加速度的白噪声。
\(R_{wb}(t)\):表示\(t\)时刻从车载坐标系到世界坐标系的旋转变换。
对于世界坐标系下的速度\(v_w(t)\)以及位置\(p_w(t)\)，根据运动方程有
(2)
$$
\begin{aligned}
\dot{v}_w(t) &= a_w(t)
\\
\dot{p}_w(t) &=v_w(t)
\\
\dot{R}_{wb}&=R_{wb}\omega_b^\wedge
\end{aligned}
$$
对上面的公式采用欧拉积分有
(3)
$$
\begin{aligned}
v_w(t+\Delta t) &= v_w(t) + a_w(t)\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}a_w(t)\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp(\omega_b(t)\Delta t)
\end{aligned}
$$
将公式(1)代入公式(3)有
(4)
$$
\begin{aligned}
v_w(t+\Delta t) &= v_w(t)+g_w(t)\Delta t+R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}g_w(t)\Delta t^2+\dfrac{1}{2}R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp((\bar{\omega}_b(t)-b^\omega(t) – \eta^\omega(t))\Delta t)
\end{aligned}
$$
这里记
(5)
$$
\begin{aligned}
R_i&=R_{wb}(t)|_{t=i}
\\
p_i&=p_w(t)|_{t=i}
\\
v_i&=v_w(t)|_{t=i}
\\
\bar{\omega}_i&=\bar{\omega}_b(t)|_{t=i}
\\
\omega_i&=\omega_b(t)|_{t=i}
\\
\bar{a}_i&=\bar{a}_b(t)|_{t=i}
\\
a_i&=a_w(t)|_{t=i}
\\
b^\omega_i&=b^\omega(t)|_{t=i}
\\
\eta^\omega_i&=\eta^\omega(t)|_{t=i}
\\
b^a_i&=b^a(t)|_{t=i}
\\
\eta^a_i&=\eta^a(t)|_{t=i}
\\
g&=g_w(t)
\\
\Delta t_{ij}&=\sum_{k=i}^{j-1} \Delta t
\end{aligned}
$$
上面假设重力加速度\(g_w(t)\)为常量。
这里定义如下增量公式
(6)
$$
\begin{aligned}
\Delta R_{ij} &=R_i^TR_j
\\
&=\prod_{k=i}^{j-1}\exp((\bar{\omega}_k-b^\omega_k – \eta^\omega_k)\Delta t)
\\
\\
\Delta v_{ij}&=v_j-v_i
\\
&=R_i^T(v_j-v_i-g\Delta t_{ij})
\\
&= \sum_{k=i}^{j-1}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t
\\
\\
\Delta p_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t_{ij}^2)
\\
&=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\dfrac{1}{2}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t^2]
\end{aligned}
$$
最终根据文献[1]得到如下预积分模型(实际使用的就是如下的模型,即如下约等于符号后的表达式)
(7)
$$
\begin{aligned}
\Delta \bar{R}_{ij}&=R_i^TR_j\exp(\delta \phi_{ij})
\\
&\approx R_i^TR_j
\\
\\
\Delta \bar{v}_{ij}&=R_i^T(v_j-v_i-g\Delta t_{ij})+\delta v_{ij}
\\
&\approx R_i^T(v_j-v_i-g\Delta t_{ij})
\\
\\
\Delta \bar{p}_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})+\delta p_{ij}
\\
&\approx R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})
\end{aligned}
$$
公式中\(\Delta \bar{R}_{ij}\)、\(\Delta \bar{v}_{ij}\)、\(\Delta \bar{p}_{ij}\)为定义的预积分测量变量，\(\delta R_{ij}\)、\(\delta v_{ij}\)、\(\delta p_{ij}\)为对应白噪声。
如果公式(1)当中的偏置不为常数，则每次需要更新预积分测量变量，这里采用如下公式更新
(8)
$$
\begin{aligned}
\Delta \bar{R}_{ij}&\approx \Delta \bar{R}_{ij} \exp (\dfrac{ \partial \Delta \bar{R}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i)
\\
\Delta \bar{v}_{ij}&\approx \Delta \bar{v}_{ij}+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^a_i} \delta b^a_i
\\
\Delta \bar{p}_{ij}&\approx \Delta \bar{p}_{ij}+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^a_i} \delta b^a_i
\end{aligned}
$$
上面公式中的偏导数可以参考文献[2].
接下来，我们可以将计算的\(\Delta \bar{R}_{ij}\)、\(\Delta \bar{v}_{ij}\)、\(\Delta \bar{p}_{ij}\)作为测量值，和外部其它变量建立约束。对相关变量进行更新，在实际优化求解中需要优化的变量为\(R_i\)、\(p_i\)、\(v_i\)、\(R_j\)、\(p_j\)、\(v_j\)、\(\delta b^\omega_i\)、\(\delta b^a_i\)。注意变量按如下方式更新
(9)
$$
\begin{aligned}
R_i &= R_i \exp(\delta \phi_i)
\\
p_i &= p_i + R_i \delta p_i
\\
v_i &= v_i + \delta v_i
\\
R_j &= R_j \exp(\delta \phi_j)
\\
p_j &= p_j + R_j \delta p_j
\\
v_j &= v_j + \delta v_j
\\
\delta b^\omega_i &= \delta b^\omega_i + \bar{\delta b^\omega_i}
\\
\delta b^a_i &= \delta b^a_i + \bar{\delta b^a_i}
\end{aligned}
$$
参考
[1] Forster C , Carlone L , Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.
[2] 苏笑云.IMU预积分的理解和推导
[3] 高翔.简明预积分推导