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} \]