gtsam官方文档: The new IMU Factor学习笔记

gtsam官方文档：The new IMU Factor

本文为gtsam imu预积分文档学习笔记，为gtsam基于切向量实现的imu预积分的理论基础。

导航状态

定义导航状态为 X b n = { R b n , P b n , V b n } X^n_b=\{R^n_b,P^n_b,V^n_b\} Xbn​={Rbn​,Pbn​,Vbn​}

• 上角标n：导航(navigation)坐标系
• 下角标b：载体(body)坐标系

下文均省略上下标

矢量场与微分方程

对于导航问题我们用如下微分方程描述 $\dot{X}(t)=F(t,X)$

• $F$ ：时变矢量场
• $\dot{X}(t)$: 一个三元组的切向量，$\dot{X}(t)=[\dot{R}(t,X),\dot{P}(t,X),\dot{V}(t,X)]\in \mathfrak{s}0(3)\times {\mathbb{R}}^3\times {\mathbb{R}}^3$
• 在$X$ 处由所有切向量组成的空间表示为 $T_{X}M$ ，即$F(t,X)\in T_{X}M$
• $\dot{R}=R[\omega {]}_X$

$$\dot{X}(t)=[\dot{R}(X,t),V(t),\dot{V}(X,t)]=\left[R(t){\left[{\omega }^b(t)\right]}_{\times },V(t),g+R(t)a^b(t)\right]$$

局部坐标

基于流形空间的优化基于局部坐标的概念。当初始位姿为$R_0$，定义一个局部映射${\Phi }_{R_0}(\theta )$从局部坐标 $\theta \in {\mathbb{R}}^3$ 映射到$R_0$ 的邻域：
$${\Phi }_{R_0}(\theta )=R_0\exp \left([\theta {]}_{\times }\right)$$
局部坐标系$\theta$ 和切向量是同构的，定义$\theta =\omega t$,有
$${\frac{d{\Phi }_{R_0}(\omega t)}{dt}|}_{t=0}={\frac{d{R}_0\exp \left([\omega t{]}_{\times }\right)}{dt}|}_{t=0}=R_0[\omega {]}_{\times }$$

• 原文这里有一个笔误，反对称矩阵里面应该没有$t$

因此，三维向量$\omega$表示了在$SO(3)$流形上的前进方向，但是仅在$R_0$附近的局部坐标系上有效。

类似的，可以定义在$SE(3)$上的局部坐标 $\xi =[\omega t,vt]\in {\mathbb{R}}^6$与其对应的映射函数
$${\Phi }_{T_0}(\xi )=T_0\exp \hat{\xi }$$

局部坐标系映射的导数

定义一个$3\times 3$的雅克比矩阵模拟增量 $\delta$ 对局部坐标系的影响。
$${\Phi }_{R_0}(\theta +\delta )\approx {\Phi }_{R_0}(\theta )\exp \left([H(\theta )\delta {]}_{\times }\right)={\Phi }_{{\Phi }_{R_0}(\theta )}(H(\theta )\delta )$$
这个雅克比的计算仅仅依赖$\theta$
$$H(\theta )=\sum _{k=0}^{\infty }\frac{(-1{)}^k}{(k+1)!}[\theta {]}_{\times }^k$$

局部坐标中的数值积分

对于问题$\dot{R}(t)=F(R,t)$,我们将其解建模为$R(t)={\Phi }_{R_0}(\theta (t))$

定义
$$\gamma (\delta )=R(t+\delta )={\Phi }_{R_0}(\theta (t)+\dot{\theta }(t)\delta )\approx {\Phi }_{R(t)}(H(\theta )\dot{\theta }(t)\delta )$$

有
$$\dot{R}(t)={\frac{d\gamma (\delta )}{d\delta }|}_{\delta =0}={\frac{d{\Phi }_{R(t)}(H(\theta )\dot{\theta }(t)\delta )}{d\delta }|}_{\delta =0}=R[t][H(\theta )\dot{\theta }(t){]}_{\times }$$

• 关于这一节将数值积分内容是导数：数值积分用欧拉法用一阶导数近似迭代计算积分

收缩

R X 0 ( ζ ) = { Φ R 0 ( ω t ) , P 0 + R 0 v t , V 0 + R 0 a t } \mathcal{R}_{X_{0}}(\zeta)=\left\{\Phi_{R_{0}}(\omega t), P_{0}+R_{0} v t, V_{0}+R_{0} a t\right\} RX0​​(ζ)={ΦR0​​(ωt),P0​+R0​vt,V0​+R0​at}

• $\zeta =[\omega t,vt,at]$
• 在$X_0$处的切向量为${\frac{d{\mathcal{R}}_{X_0}(\zeta )}{dt}|}_{t=0}=\left[R_0[\omega {]}_{\times },R_{0}v,R_{0}a\right]$

基于局部坐标的积分

将状态表示为时间的函数
X ( t ) = R X 0 ( ζ ( t ) ) = { Φ R 0 ( θ ( t ) ) , P 0 + R 0 p ( t ) , V 0 + R 0 v ( t ) } X(t)=\mathcal{R}_{X_{0}}(\zeta(t))=\left\{\Phi_{R_{0}}(\theta(t)), P_{0}+R_{0} p(t), V_{0}+R_{0} v(t)\right\} X(t)=RX0​​(ζ(t))={ΦR0​​(θ(t)),P0​+R0​p(t),V0​+R0​v(t)}

定义一个轨迹函数$\gamma (\delta )$，自变量为时间的增量$\delta$,每一个$\delta$对应状态空间的条轨迹，$\gamma (\delta )$表示轨迹簇，并在$\delta =0$时通过X(t):
γ ( δ ) = X ( t + δ ) = { Φ R 0 ( θ ( t ) + θ ˙ ( t ) δ ) , P 0 + R 0 { p ( t ) + p ˙ ( t ) δ } , V 0 + R 0 { v ( t ) + v ˙ ( t ) δ } } \gamma(\delta)=X(t+\delta)=\left\{\Phi_{R_{0}}(\theta(t)+\dot{\theta}(t) \delta), P_{0}+R_{0}\{p(t)+\dot{p}(t) \delta\}, V_{0}+R_{0}\{v(t)+\dot{v}(t) \delta\}\right\} γ(δ)=X(t+δ)={ΦR0​​(θ(t)+θ˙(t)δ),P0​+R0​{p(t)+p˙​(t)δ},V0​+R0​{v(t)+v˙(t)δ}}

• ${\Phi }_{R_0}(\theta (t+\delta ))={\Phi }_{R_0}(\theta (t)+\dot{\theta }(t)\delta )$
  • 这一步骤是用一阶导数近似，后面两项相同

导航状态的导数可以表示为

$$\dot{X}(t)={\frac{d\gamma (\delta )}{d\delta }|}_{\delta =0}=\left[R(t){\left[H(\theta )\dot{\theta }(t)\right]}_{\times },R_0\dot{p}(t),R_0\dot{v}(t)\right]$$

应用：新的IMU因子

铺垫了这么多 终于进入核心内容了

在Lupton 的论文(On-Manifold Preintegration for Real-Time Visual–Inertial Odometry)中，当不知道导航状态$X_i$的状态时，无法对重力加速度进行补偿，因此吧速度拆成重力加速和比力两部分。

• 注：IMU测的加速度实际上是比力(Specific force)–$f^b$
  • $f^b=R_n^b(a_{ii}^n-g^n)$
  • 我们真正需要的加速度是相对于惯性系的线性加速度 $a_{ii}^n=R_b^n{f}^b+g^n$
  • 要对重力进行补偿就需要知道载体坐标系相对导航坐标系的相对位姿

对速度项做拆分

• Lupton 原文的预积分拆法表达

$$v_{t2}^n=v_{t1}^n+{\int }_{t1}^{t2}\left(R_{bt}^n\left(f_t^b-bia{s}_f^{obs}\right)+g^n\right)dt=v_{t1}^n+{\int }_{t1}^{t2}{g}^{n}dt+R_{bt1}^n{\int }_{t1}^{t2}\left(R_{bt}^{bt1}\left(f_t^b-bia{s}_f^{obs}\right)\right)dt$$

​ 把原文这个式子左右两边同乘$R_n^{bt1}$有
$$v_{t2}^{bt1}=v_{t1}^{bt}+R_n^{bt1}{\int }_{t1}^{t2}{g}^{n}dt+{\int }_{t1}^{t2}\left(R_{bt}^{bt1}\left(f_t^b-bia{s}_f^{obs}\right)\right)dt\text{\hspace{1em}(1)}$$

• 本文档的表达 $v(t)=v_g(t)+v_a(t)$

  进一步的

$$\begin{array}{rl}& {\dot{v}}_g(t)=R_i^{T}g\\ & {\dot{v}}_a(t)=R_b^i(t)a^b(t)\end{array}$$

​ 这里的公式和$(1)$中相同。

对位置项做拆分

• Lupton原文的拆法

$$p_{t2}^n=p_{t1}^n+(t2-t1)v_{t1}^n+{\iint }_{t1}^{t2}\left(R_{bt}^n\left(f_t^b-bia{s}_f^{obs}\right)+g^n\right)d{t}^2=p_{t1}^n+(t2-t1)v_{t1}^n+{\iint }_{t1}^{t2}{g}^{n}d{t}^2+R_{bt1}^n{\iint }_{t1}^{t2}\left(R_{bt}^{bt1}\left(f_t^b-bia{s}_f^{obs}\right)\right)d{t}^2$$

​ 把原文这个式子左右两边同乘$R_n^{bt1}$有
$$p_{t2}^{bt1}=p_{t1}^{bt1}+(t2-t1)v_{t1}^{bt1}+R_n^{bt1}{\iint }_{t1}^{t2}{g}^{n}d{t}^2+{\iint }_{t1}^{t2}\left(R_{bt}^{bt1}\left(f_t^b-bia{s}_f^{obs}\right)\right)d{t}^2\text{\hspace{1em}(2)}$$

• gtsam文档拆法表达

$$\begin{array}{rl}& p(t)=p_i(t)+p_g(t)+p_v(t)\\ & {\dot{p}}_i(t)=R_i^T{V}_i\\ & {\dot{p}}_g(t)=v_g(t)=R_i^{T}gt\\ & {\dot{p}}_v(t)=v_a(t)\end{array}$$

​ 这个式子和$(2)$本质相同。

总的来说，IMU预积分因子由一下三项组成
$$\begin{array}{rl}\dot{\theta }(t)& =H(\theta (t){)}^{-1}{\omega }^b(t)\\ {\dot{p}}_v(t)& =v_a(t)\\ {\dot{v}}_a(t)& =R_b^i(t)a^b(t)\end{array}$$
从零开始，一直到时间$t_{ij}$，有$R_b^i(t)=\text{exp}([\theta (t){]}_{\times })$,用局部坐标向量表示为
$$\zeta \left(t_{ij}\right)=\left[\theta \left(t_{ij}\right),p\left(t_{ij}\right),v\left(t_{ij}\right)\right]=\left[\theta \left(t_{ij}\right),R_i^T{V}_i{t}_{ij}+R_i^T\frac{g{t}_{ij}^2}{2}+p_v\left(t_{ij}\right),R_i^{T}g{t}_{ij}+v_a\left(t_{ij}\right)\right]$$
$X_j$的导航状态可以表达为
$$X_j={\mathcal{R}}_{X_i}\left(\zeta \left(t_{ij}\right)\right)=\left\{{\Phi }_{R_0}\left(\theta \left(t_{ij}\right)\right),P_i+V_i{t}_{ij}+\frac{g{t}_{ij}^2}{2}+R_i{p}_v\left(t_{ij}\right),V_i+g{t}_{ij}+R_i{v}_a\left(t_{ij}\right)\right\}$$

用欧拉法求解微分方程

欧拉法求解微分方程的原理就是用一阶导数近似来计算积分，有以下三个式子

$$\begin{array}{rl}{\theta }_{k+1}={\theta }_k+\dot{\theta }\left(t_k\right){\Delta }_t& ={\theta }_k+H{\left({\theta }_k\right)}^{-1}{\omega }_k^b{\Delta }_t\\ p_{k+1}=p_k+{\dot{p}}_v\left(t_k\right){\Delta }_t& =p_k+v_k{\Delta }_t\\ v_{k+1}=v_k+{\dot{v}}_a\left(t_k\right){\Delta }_t& =v_k+\exp \left({\left[{\theta }_k\right]}_{\times }\right)a_k^b{\Delta }_t\end{array}$$
其中${\theta }_k\triangleq \theta \left(t_k\right),p_k\triangleq p_v\left(t_k\right),v_k\triangleq v_a\left(t_k\right)$

进一步提高位置的精度，引入二阶导数计算(加速度)

$$\begin{array}{rl}{\theta }_{k+1}& ={\theta }_k+H{\left({\theta }_k\right)}^{-1}{\omega }_k^b{\Delta }_t\\ p_{k+1}& =p_k+v_k{\Delta }_t+R_k{a}_k^b\frac{{\Delta }_t^2}{2}\\ v_{k+1}& =v_k+R_k{a}_k^b{\Delta }_t\end{array}$$

噪声(协方差矩阵)传递

定义$t_k$时刻的导航状态为${\zeta }_k=[{\theta }_k,p_k,v_k]$，
$${\zeta }_{k+1}=f\left({\zeta }_k,a_k^b,{\omega }_k^b\right)$$
噪声的传递可以表达为
$${\Sigma }_{k+1}=A_k{\Sigma }_k{A}_k^T+B_k{\Sigma }_{\eta }^{ad}{B}_k+C_k{\Sigma }_{\eta }^{gd}{C}_k$$

• ${\Sigma }_{k+1}$: $t_{k+1}$时刻的协方差矩阵，$9\times 9$大小

• ${\Sigma }_k$: $t_k$时刻的协方差矩阵，$9\times 9$矩阵

• ${\Sigma }_{\eta }^{ad}$测量加速度引入的噪声，$3\times 3$矩阵

• ${\Sigma }_{\eta }^{gd}$测量角速度引入的噪声，$3\times 3$矩阵

• $$A_k\approx \left[\begin{array}{ccc}{I}_{3\times 3}-\frac{{\Delta }_t}{2}{\left[{\omega }_k^b\right]}_{\times }& 0& 0\\ R_k{\left[-a_k^b\right]}_{\times }H\left({\theta }_k\right)\frac{{\Delta }_t}{2}& I_{3\times 3}& I_{3\times 3}{\Delta }_t\\ R_k{\left[-a_k^b\right]}_{\times }H\left({\theta }_k\right){\Delta }_t& 0& I_{3\times 3}\end{array}\right]$$

• $$B_k=\left[\begin{array}{c}{0}_{3\times 3}\\ R_k\frac{{\Delta }_t}{2}\\ R_k{\Delta }_t\end{array}\right],C_k=\left[\begin{array}{c}H{\left({\theta }_k\right)}^{-1}{\Delta }_t\\ 0_{3\times 3}\\ 0_{3\times 3}\end{array}\right]$$