[VINS-Mono]IMU预积分

IMU模型

$$\begin{array}{c}{\hat{a}}_b=q_{bw}\left(a^w+g^w\right)+b_a+n_a\\ {\hat{\omega }}_b={\omega }_b+b_{\omega }+n_{\omega }\end{array}$$
其中：

• $b$是body坐标系, $w$是世界坐标系
• $\widehat{(\cdot )}$ 是测量值，都在body坐标系下
• $n_a$, $n_{\omega }$为加速度和陀螺仪测量中的附加噪声，其为高斯噪声：$n_a\sim N\left(0,{\sigma }_a^2\right),n_b\sim N\left(0,{\sigma }_b^2\right)$
• $b_a$, $b_{\omega }$ 是加速度和陀螺仪的偏置，其为随机游走，其导数为高斯：
  $$\begin{array}{c}{n}_{b_a}\sim N\left(0,{\sigma }_{b_a}^2\right),n_{b_{\omega }}\sim N\left(0,{\sigma }_{b_{\omega }}^2\right)\\ b_a^{\prime }=n_{b_a},b_{\omega }^{\prime }=n_{b_{\omega }}\end{array}$$

当前时刻的PVQ

1.当前时刻的PVQ的连续形式

将第$k$帧和第$k+1$帧之间的所有IMU进行积分，可以得到第k+1帧的位置、速度以及旋转
$$\{\begin{array}{ccccc}\dot{q}& =& q\otimes {\left[0,\frac{1}{2}w\right]}^T& =& \frac{1}{2}\Omega \left(w\right)q\\ \dot{V}& =& a& & \\ \dot{P}& =& V& & \end{array}\stackrel{integration}{⟹}\{\begin{array}{ccc}q\left(t+\Delta t\right)& =& q\left(t\right)\otimes {\int }_{\left[t,t+\Delta t\right]}\frac{1}{2}\Omega \left(w\right)qdt\\ V\left(t+\Delta t\right)& =& V\left(t\right)+{\int }_{\left[t,t+\Delta t\right]}adt\\ p\left(t+\Delta t\right)& =& p\left(t\right)+{\int }_{\left[t,t+\Delta t\right]}\left(V\left(t\right)+{\int }_{\left[t,t+\Delta t\right]}adt\right)dt\end{array}$$
将IMU的测量模型代入，得到连续形式：
$$\begin{array}{l}{\mathbf{p}}_{b_{k+1}}^w={\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k+{\iint }_{t\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_t^w\left({\hat{a}}_t-b_{a_t}-n_a\right)-g^w\right)d{t}^2\\ {\mathbf{v}}_{b_{k+1}}^w={\mathbf{v}}_{b_k}^w+{\int }_{t\in \left[t_k,t_{k+1}\right]}\left({\mathbf{R}}_t^w\left({\hat{a}}_t-b_{a_t}-n_a\right)-g^w\right)dt\\ {\mathbf{q}}_{b_{k+1}}^w={\mathbf{q}}_{b_k}^w\otimes {\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\Omega \left({\hat{w}}_t-b_{w_t}-n_w\right){\mathbf{q}}_t^{b_k}dt\end{array}$$

其中：
Ω = [ − [ ω ] × ω − ω T 0 ] , [ ω ] × = [ 0 − ω z ω y ω z 0 ω x − ω y ω x 0 ] \Omega=\left[\begin{array}{cc} -[\omega]_{\times} & \omega \\ -\omega^{T} & 0 \end{array}\right],[\omega]_{\times}=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & \omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right] Ω=[−[ω]×​−ωT​ω0​],[ω]×​= ​0ωz​−ωy​​−ωz​0ωx​​ωy​ωx​0​ ​
其中，$w={\hat{w}}_t-b_{w_t}-n_w$

2.当前时刻PVQ的离散形式

进行中值积分，然后得到i+1时刻的位置、速度以及旋转(PVQ)
中值积分：
$$\begin{array}{l}{\overline{\hat{a}}}_i=\frac{1}{2}\left[{\mathbf{q}}_i\left({\hat{a}}_i-b_{a_i}\right)-g^w+{\mathbf{q}}_{i+1}\left({\hat{a}}_{i+1}-b_{a_i}\right)-g^w\right].\\ {\overline{\hat{w}}}_i=\frac{1}{2}\left({\hat{w}}_i+{\hat{w}}_{i+1}\right)-b_{w_i}\end{array}$$
离散形式：
$$\begin{array}{l}{\mathbf{p}}_{b_{i+1}}^w={\mathbf{p}}_{b_i}^w+{\mathbf{v}}_{b_i}^w\delta t+\frac{1}{2}{\overline{\hat{a}}}_i\delta t^2\\ {\mathbf{v}}_{b_{i+1}}^w={\mathbf{v}}_{b_i}^w+{\overline{\hat{a}}}_i\delta t\\ {\mathbf{q}}_{b_{i+1}}^w={\mathbf{q}}_{b_i}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\overline{\hat{w}}}_i\delta t\end{array}\right]\end{array}$$

两帧之间的PVQ增量

1.两帧之间PVQ增量的连续形式

优化的变量为：
$$x_k=\left[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g\right]$$
IMU的积分构建为帧间约束，其次P和V与第$k$时刻的状态有关，做优化时需要迭代更新第k时刻的状态，导致需要每次重新计算积分，为了不要使IMU积分量与优化变量有关，因此将积分从世界坐标系变换到第k帧的载体坐标系，公式两边左右各乘$R_w^{b_k}$，从而得到:
$$\begin{array}{c}{\mathbf{R}}_w^{b_k}{\mathbf{p}}_{b_{k+1}}^w={\mathbf{R}}_w^{b_k}\left({\mathbf{p}}_{b_k}^w+{\mathbf{v}}_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2\right)+{\alpha }_{b_{k+1}}^{b_k}\\ {\mathbf{R}}_w^{b_k}{\mathbf{v}}_{b_{k+1}}^w={\mathbf{R}}_w^{b_k}\left({\mathbf{v}}_{b_k}^w-g^w\Delta t_k\right)+{\beta }_{b_{k+1}}^{b_k}\\ {\mathbf{q}}_w^{b_k}\otimes {\mathbf{q}}_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}\end{array}$$
其中：
$$\begin{array}{rl}{\alpha }_{b_{k+1}}^{b_k}& ={\iint }_{t\in \left[t_k,t_{k+1}\right]}{\mathbf{R}}_t^{b_k}\left({\hat{a}}_t-b_{a_t}-n_a\right)d{t}^2\\ {\beta }_{b_{k+1}}^{b_k}& ={\int }_{t\in \left[t_k,t_{k+1}\right]}{\mathbf{R}}_t^{b_k}\left({\hat{a}}_t-b_{a_t}-n_a\right)dt\\ {\gamma }_{b_{k+1}}^{b_k}& ={\int }_{t\in \left[t_{k,},t_{k+1}\right]}\frac{1}{2}\Omega \left({\hat{w}}_t-b_{w_t}-n_w\right){\gamma }_t^{b_k}dt\end{array}$$
预积分项仅与IMU测量有关，而与被优化的状态量无关，此时的${\alpha }_{b_{k+1}}^{b_k}$, ${\beta }_{b_{k+1}}^{b_k}$, ${\gamma }_{b_{k+1}}^{b_k}$可以视为$b_{k+1}$与$b_k$之间的相对运动，$b_k$​的状态改变并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递

2.两帧之间PVQ增量的离散形式

• 离散时间预积分的单步PVQ传播(预积分更新)：
  $$\begin{array}{l}{\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}{\bar{a}}_i^{\prime }\delta t^2\\ {\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+{\overline{\hat{a}}}_i^{\prime }\delta t\\ {\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\hat{w}}_i^{\prime }\delta t\end{array}\right]\end{array}$$
• 欧拉法
  $$\begin{array}{l}{a}_i^{\prime }=\mathbf{R}\left({\hat{\gamma }}_i^{b_k}\right)\left({\hat{a}}_i-b_{a_i}\right)\\ {\hat{w}}_i^{\prime }={\hat{w}}_i-b_{w_i}\end{array}$$
• 中值法
  $$\begin{array}{l}{a}_i^{\prime }=\frac{1}{2}\left[{\mathbf{q}}_i\left({\hat{a}}_i-b_{a_i}\right)+{\mathbf{q}}_{i+1}\left({\hat{a}}_{i+1}-b_{a_i}\right)\right]\\ {\hat{\hat{w}}}_i^{\prime }=\frac{1}{2}\left({\hat{w}}_i+{\hat{w}}_{i+1}\right)-b_{w_i}\end{array}$$

注意：

• 前后两帧之间IMU增量信息
  $$\begin{array}{l}{\hat{\alpha }}_{i+1}^{b_k}={\hat{\alpha }}_i^{b_k}+{\hat{\beta }}_i^{b_k}\delta t+\frac{1}{2}{\bar{a}}_i^{\prime }\delta t^2\\ {\hat{\beta }}_{i+1}^{b_k}={\hat{\beta }}_i^{b_k}+{\overline{\hat{a}}}_i^{\prime }\delta t\\ {\hat{\gamma }}_{i+1}^{b_k}={\hat{\gamma }}_i^{b_k}\otimes {\hat{\gamma }}_{i+1}^i={\hat{\gamma }}_i^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\hat{w}}_i^{\prime }\delta t\end{array}\right]\end{array}$$
• 当前帧时刻的物理量信息
  $$\begin{array}{l}{\mathbf{p}}_{b_{i+1}}^w={\mathbf{p}}_{b_i}^w+{\mathbf{v}}_{b_i}^w\delta t+\frac{1}{2}{\overline{\hat{a}}}_i\delta t^2\\ {\mathbf{v}}_{b_{i+1}}^w={\mathbf{v}}_{b_i}^w+{\overline{\hat{a}}}_i\delta t\\ {\mathbf{q}}_{b_{i+1}}^w={\mathbf{q}}_{b_i}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\overline{\hat{w}}}_i\delta t\end{array}\right]\end{array}$$

预积分更新

${\alpha }_{b_{k+1}}^{b_k}$, ${\beta }_{b_{k+1}}^{b_k}$, ${\gamma }_{b_{k+1}}^{b_k}$与IMU的bais有关，而bais也是需要优化的变量，这将导致的问题是：当每次迭代时，我们得到一个新的bais ，还需要根据预积分项重新对第k帧和第k+1帧之间的IMU预积分，非常耗时
解决方法：当bias变换很小时，对bias进行一阶近似调整${\alpha }_{b_{k+1}}^{b_k}$, ${\beta }_{b_{k+1}}^{b_k}$, ${\gamma }_{b_{k+1}}^{b_k}$，避免重新计算，但是在预积分的过程中还要逐步计算其误差的雅可比矩阵
$$\begin{array}{l}{\alpha }_{b_{k+1}}^{b_k}\approx {\hat{\alpha }}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\alpha }\delta b_{a_k}+{\mathbf{J}}_{b_w}^{\alpha }\delta b_{w_k}\\ {\beta }_{b_{k+1}}^{b_k}\approx {\hat{\beta }}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\beta }\delta b_{a_k}+{\mathbf{J}}_{b_w}^{\beta }\delta b_{w_k}\\ {\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta b_{w_k}\end{array}\right]\end{array}$$
其中：
$$J_{b_u}^{\alpha }=\frac{\delta {\alpha }_{b_k}^{b_{k+1}}}{\delta b_{a_k}},J_{b_{\omega }}^{\alpha }=\frac{\delta {\alpha }_{b_k}^{b_{k+1}}}{\delta b_{{\omega }_k}},J_{b_{\alpha }}^{\beta }=\frac{\delta {\beta }_{b_k}^{b_{k+1}}}{\delta b_{a_k}},J_{b_{\omega }}^{\beta }=\frac{\delta {\beta }_{b_k}^{b_{k+1}}}{\delta b_{{\omega }_k}},J_{b_{\omega }}^{\gamma }=\frac{\delta {\gamma }_{b_k}^{b_{k+1}}}{\delta b_{{\omega }_k}}$$