VINS中IMU预积分

连续时间IMU积分

\(b_k\)和\(b_{k+1}\)为相邻关键帧，

\[\begin{aligned} &\mathbf{p}_{b_{k+1}}^w=\mathbf{p}_{b_k}^w+\mathbf{v}_{b_k}^w\Delta t_k+\iint_{t\in[t_k,t_{k+1}]}\left(\mathbf{R}_t^w(\hat{\mathbf{a}}_t-\mathbf{b}_{a_t}-\mathbf{n}_a)-\mathbf{g}^w\right)dt^2\ \\ &\mathbf{v}_{b_{k+1}}^w=\mathbf{v}_{b_k}^w+\int_{t\in[t_k,t_{k+1}]}(\mathbf{R}_t^w(\hat{\mathbf{a}}_t-\mathbf{b}_{a_t}-\mathbf{n}_a)-\mathbf{g}^w)dt\\ &\mathbf{q}_{b_{k+1}}^w=\mathbf{q}_{b_k}^w\otimes\int_{t\in[t_k,t_{k+1}]}\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}}_t-\mathbf{b}_{w_t}-\mathbf{n}_w)\mathbf{q}_t^{b_k}dt\\ &其中\\ &\boldsymbol{\Omega}(\boldsymbol{\omega})=\begin{bmatrix}-\lfloor\boldsymbol{\omega}\rfloor_\times&\boldsymbol{\omega}\\-\boldsymbol{\omega}^T&0\end{bmatrix}, \lfloor\boldsymbol{\omega}\rfloor_\times=\begin{bmatrix}0&-\omega_z&\omega_y\\\omega_z&0&-\omega_x\\-\omega_y&\omega_x&0\end{bmatrix} \end{aligned} \]

从世界坐标系\(w\)转到\(b_k\)帧下

\[\begin{aligned} \mathbf{R}_w^{b_k}\mathbf{p}_{b_{k+1}}^w& =\mathbf{R}_w^{b_k}(\mathbf{p}_{b_k}^w+\mathbf{v}_{b_k}^w\Delta t_k-\frac12\mathbf{g}^w\Delta t_k^2)+\boldsymbol{\alpha}_{b_{k+1}}^{b_k} \\ \mathbf{R}_w^{b_k}\mathbf{v}_{b_{k+1}}^w& =\mathbf{R}_w^{b_k}(\mathbf{v}_{b_k}^w-\mathbf{g}^w\Delta t_k)+\boldsymbol{\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{aligned} \]

连续时间预积分

\[\begin{aligned} \alpha_{b_{k+1}}^{b_{k}} &=\iint_{t\in[t_k,t_{k+1}]}\mathbf{R}_t^{b_k}(\hat{\mathbf{a}}_t-\mathbf{b}_{a_t}-\mathbf{n}_a)dt^2 \\ \boldsymbol{\beta}_{bk+1}^{bk} &=\int_{t\in[t_k,t_{k+1}]}\mathbf{R}_t^{b_k}(\hat{\mathbf{a}}_t-\mathbf{b}_{a_t}-\mathbf{n}_a)dt \\ \gamma_{bk+1}^{bk} &=\int_{t\in[t_k,t_{k+1}]}\frac12\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}}_t-\mathbf{b}_{w_t}-\mathbf{n}_w)\boldsymbol{\gamma}_t^{b_k}dt \end{aligned} \]

离散时间下的预积分

预积分递推如下，\(i\)是时间\([t_k, t_{k+1}]\)之间IMU测量时刻，起始时\(\boldsymbol{\alpha}_{b_k}^{b_k}, \boldsymbol{\beta}_{b_k}^{b_k}\)是\(\textbf{0}\), \(\boldsymbol{\gamma}_{b_k}^{b_k}\)是单位四元数，噪声\(n_a,n_w\)未知设为0，预积分为估计值以\(\hat{(\cdot)}\)表示。

\[\begin{aligned} &\hat{\boldsymbol{\alpha}}_{i+1}^{b_k} =\hat{\boldsymbol{\alpha}}_i^{b_k}+\hat{\boldsymbol{\beta}}_i^{b_k}\delta t+\frac12\mathbf{R}(\hat{\boldsymbol{\gamma}}_i^{b_k})(\hat{\mathbf{a}}_i-\mathbf{b}_{a_i})\delta t^2 \\ &\hat{\boldsymbol{\beta}}_{i+1}^{b_k} =\hat{\boldsymbol{\beta}}_i^{b_k}+\mathbf{R}(\hat{\boldsymbol{\gamma}}_i^{b_k})(\hat{\mathbf{a}}_i-\mathbf{b}_{a_i})\delta t \\ &\hat{\boldsymbol{\gamma}}_{i+1}^{b_k} =\hat{\boldsymbol{\gamma}}_i^{b_k}\otimes\begin{bmatrix}1\\\frac12(\hat{\boldsymbol{\omega}}_i-\mathbf{b}_{w_i})\delta t\end{bmatrix} \end{aligned} \]

连续时间预积分误差线性化

参考误差卡尔曼滤波推导误差\(\delta\mathbf{z}_t^{b_k}\)对时间t的一阶导

\[\begin{aligned} \dot{\delta\mathbf{z}_t^{b_k}} = \begin{bmatrix}\delta\dot{\boldsymbol{\alpha}}_t^{b_k}\\\delta\dot{\boldsymbol{\beta}}_t^{b_k}\\\delta\dot{\boldsymbol{\theta}}_t\\\delta\dot{\boldsymbol{b}}_{a_t}\\\delta\dot{\boldsymbol{b}}_{w_t}\end{bmatrix}& =\begin{bmatrix}0&\mathbf{I}&0&0&0\\0&0&-\mathbf{R}_t^{b_k}\lfloor\hat{\mathbf{a}}_t-\mathbf{b}_{a_t}\rfloor_\times&-\mathbf{R}_t^{b_k}&0\\0&0&-\lfloor\hat{\boldsymbol{\omega}}_t-\mathbf{b}_{w_t}\rfloor_\times&0&-\mathbf{I}\\0&0&0&0&0\\0&0&0&0&0\end{bmatrix}\begin{bmatrix}\delta\boldsymbol{\alpha}_t^{b_k}\\\delta\boldsymbol{\beta}_t^{b_k}\\\delta\boldsymbol{\theta}_t^{b_k}\\\delta\mathbf{b}_{a_t}\\\delta\mathbf{b}_{w_t}\end{bmatrix}+\begin{bmatrix}0&0&0&0\\-\mathbf{R}_t^{b_k}&0&0&0\\0&-\mathbf{I}&0&0\\0&0&\mathbf{I}&0\\0&0&0&\mathbf{I}\end{bmatrix}\begin{bmatrix}\mathbf{n}_a\\\mathbf{n}_w\\\mathbf{n}_{b_a}\\\mathbf{n}_{b_w}\end{bmatrix}=\mathbf{F}_t\delta\mathbf{z}_t^{b_k}+\mathbf{G}_t\mathbf{n}_t \end{aligned} \]

预积分误差递推

\[\begin{aligned} \delta\mathbf{z}_{t+\delta t}^{b_k}&= \delta\mathbf{z}_t^{b_k} + \dot{\delta\mathbf{z}_t^{b_k}}\delta t \\ &=(\mathbf{I}+\mathbf{A}_t\delta t)\delta\mathbf{z}_t^{b_k}+G_t \delta t n_t\\ &=F\delta\mathbf{z}_t^{b_k}+Vn_t\\ & t\in[k,k+1] \end{aligned} \]

预积分递推细化

通过观测的加速度和加速度细化递推公式

\[\begin{aligned} \delta z_{t+\delta t}&=F_t\delta\mathbf{z}_t+V_t n_t\\ &=\begin{bmatrix}\delta\alpha_{t+\delta t}\\\delta\theta_{t+\delta t}\\\delta\beta_{t+\delta t}\\\delta b_{at+\delta t}\\\delta b_{wt+\delta t}\end{bmatrix} =\begin{bmatrix}I&f_{01}&\delta t&f_{03}&f_{04}\\0&f_{11}&0&0&-\delta t\\0&f_{21}&I&f_{23}&f_{24}\\0&0&0&I&0\\0&0&0&0&I\end{bmatrix}\begin{bmatrix}\delta\alpha_t\\\delta\theta_t\\\delta\beta_t\\\delta b_{at}\\\delta b_{wt}\end{bmatrix}+\begin{bmatrix}v_{00}&v_{01}&v_{02}&v_{03}&0&0\\0&\frac{-\delta t}{2}&0&\frac{-\delta t}{2}&0&0\\\frac{-R_t\delta t}{2}&v_{21}&\frac{-R_{t+\delta t}\delta t}{2}&v_{23}&0&0\\0&0&0&0&\delta t&0\\0&0&0&0&0&\delta t\end{bmatrix}\begin{bmatrix}n_{at}\\n_{wt}\\n_{at+\delta t}\\n_{at+\delta t}\\n_{ba}\\n_{bw}\end{bmatrix}\\ f_{01}& =\frac{\delta t}{2}f_{21}=-\frac{1}{4}R_{k}\big(\hat{a}_{k}-b_{a_{k}}\big)^{\wedge}\delta t^{2}-\frac{1}{4}R_{k+1}\big(\hat{a}_{k+1}-b_{a_{k}}\big)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge}\delta t\right]\delta t^{2} \\ f_{03}& =-\frac14(R_k+R_{k+1})\delta t^2 \\ f_{04}& =\frac{\delta t}{2}f_{24}=\frac{1}{4}R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\delta t^{3} \\ f_{11}&=I-\left(\frac{\widehat{\omega}_k+\widehat{\omega}_{k+1}}2-b_{\omega_k}\right)^\wedge\delta t\\ f_{21}&=-\frac{1}{2}R_{k}\big(\hat{a}_{k}-b_{a_{k}}\big)^{\wedge}\delta t-\frac{1}{2}R_{k+1}\big(\hat{a}_{k+1}-b_{a_{k}}\big)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge}\delta t\right]\delta t \\ f_{23}& =-\frac{1}{2}(R_{k}+R_{k+1})\delta t\quad f_{24}=\frac{1}{2}R_{k+1}\big(\hat{a}_{k+1}-b_{a_{k}}\big)^{\wedge}\delta t^{2} \\ v_{00}& =-\frac14R_{k}\delta t^{2} \\ v_{01}& =v_{03}=\frac{\delta t}{2}v_{21}=\frac{1}{4}R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\delta t^{2}\frac{\delta t}{2} \\ v_{02}& =-\frac14R_{k+1}\delta t^2 \\ v_{21}& =v_{23}=\frac{1}{4}R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\delta t^{2} \end{aligned}\]

预积分协方差

起始协方差\(\mathbf{P}_{b_k}^{b_k}\)为是\(\textbf{0}\)，按照imu频率叠加可以计算出预积分协方差\(\mathbf{P}_{b_k}^{b_k+1}\)

\[\begin{aligned} \mathbf{P}_{t+\delta t}^{b_k}&=F_t\mathbf{P}_t^{b_k}{F_t}^T+V_t\mathbf{Q}{V_t}^T\\&t\in[k,k+1] \end{aligned} \]

bais变化——>预积分变化

bais变化会导致预积分变化，为节省反复重新积分的计算量，可以通过一阶线性化来近似计算。

\[\begin{aligned} &\alpha_{b_{k+1}}^{b_k} \approx\hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_k}+\mathbf{J}_{b_a}^\alpha\delta\mathbf{b}_{a_k}+\mathbf{J}_{b_w}^\alpha\delta\mathbf{b}_{w_k} \\ &\beta_{bk+1}^{bk} \approx\hat{\beta}_{b_{k+1}}^{b_k}+\mathbf{J}_{b_a}^\beta\delta\mathbf{b}_{a_k}+\mathbf{J}_{b_w}^\beta\delta\mathbf{b}_{w_k} \\ &\gamma_{bk+1}^{bk} \approx\hat{\gamma}_{b_{k+1}}^{b_k}\otimes\begin{bmatrix}1\\\frac12\mathbf{J}_{b_w}^\gamma\delta\mathbf{b}_{w_k}\end{bmatrix} \end{aligned} \]

预积分对bais的一阶导

\(\mathbf{J}_{b_{k+1}}^{b_k}\) 是 \(\mathbf{z}_{b_{k+1}}^{b_k}\) 对 \(\mathbf{z}_{b_k}^{b_k}\) 一阶导数，其中只用到预积分量相对于bais的一阶导，\(\mathbf{J}_{b_a}^\alpha\), \(\mathbf{J}_{b_w}^\alpha\)，\(\mathbf{J}_{b_a}^\beta\),\(\mathbf{J}_{b_w}^\beta\),\(\mathbf{J}_{b_w}^\gamma\)

\[\begin{aligned} \mathbf{J}_{b_{k+1}}^{b_k}&=\frac{\delta\mathbf{z}_{b_{k+1}}^{b_k}}{\delta\mathbf{z}_{b_k}^{b_k}}=\prod_{t=b_k}^{b_{k+1}}\frac{\delta\mathbf{z}_{t+\delta t}^{b_k}}{\delta\mathbf{z}_t^{b_k}} =\prod_{t=b_k}^{b_{k+1}}F_t \end{aligned} \]