计算机视觉资料 SLM_VIO学习总结资料 预积分Foster NoteOnIMUPreintegrtion_y谢晓佳

IMU Preintegration on Manifold Zerolover June 29 2016 1 1SO 3 Group 关于 SO 3 的介绍略过 这里只列出几个近似的公式 Exp I 1 Exp Exp Exp Jr 2 Exp Exp Exp J 1 r 3 Exp Exp Exp J 1 r Exp 4 以及 Adjoint 表示 Exp R RExp RT 5 2IMU Preintegration measurements 2 1Integration measurements 给定初值 在 i 和 j 时刻对 imu 的角速度和加速度进行积分 可以计算 j 时刻相对于 i 时刻的姿态 Rj Ri j 1 k i Exp wk bg i gd k t vj vi j 1 k i g Rk ak ba i ad k t pj pi j 1 k i vk t 1 2 j 1 k i g Rk ak ba i ad k t2 6 2 2Preintegration measurements 在 preintegration 理论中需要将初值 Ri vi pi 和常数项 包含重力 g 的项 分离出来 Rij RT iRj j 1 k i Exp wk bg i gd k t vij RT i vj vi g t j i j 1 k i Rik ak ba i ad k t pij RT i pj pi vi t j i 1 2g t 2 j i 2 j 1 k i vik t 1 2 Rik ak ba i ad k t2 7 Rij vij pij即为 preintegration measurements 即不考虑初值以及重力加速度项的相对测量 注意到 这些项包含有噪声 我们也需要将它们分离出来 在分离的过程中发现 preintegration measurements 是近 似服从高斯分布的 即 Rij RijExp ij vij vij vij pij pij pij 8 2 其中 Rij vij pij为我们可以计算的测量值 不包含噪声 Rij j 1 k i Exp wk bg i t vij j 1 k i Rik ak ba i t pij j 1 k i vik t 1 2 Rik ak ba i t 2 9 定义 ij T ij pTij vTij T9 1 N 09 1 ij 为 noise preintegration vector 它们是和噪声 相关的项 这里不会对 ij 进行求解 因为事实上我们仅需要其递推形式 2 3Iterative preintegration measurements 首先给出包含噪声的递推公式 Ri k 1 Ri kExp wk bg i gd k t vi k 1 vi k Ri k ak ba i ad k t pi k 1 pi k vi k t 1 2 Ri k ak ba i ad k t2 10 接着给出不含噪声的递推公式 Ri k 1 Ri kExp wk bg i t vi k 1 vi k Ri k ak ba i t pi k 1 pi k vi k t 1 2 Ri k ak ba i t 2 11 3IMU Preintegration Noise Propagation and Bias Updates 3 1Iterative Noise Propagation 在前面提到 noise preintegration vector ij T ij pTij vTij T N 09 1 ij 这里将证明 preintegration measurements 近似服从高斯分布 并给出 ij 的递推计算结果 Rotation 根据 SO 3 中不确定性的定义 有 Rk k 1 Rk k 1Exp k k 1 Rk k 1表示包含 bias 和 noise 两个相邻时刻的相对旋转 Rk k 1表示不包含 noise 两个相邻时刻的相对 旋转 Rk k 1 Exp wk bg i gd k t 2 Exp wk bg i t Exp Jr wk b g i t gd k t Rk k 1Exp Jk r gd t Rk k 1Exp k k 1 12 其中 Rk k 1 Exp wk bg i t J k r Jr wk bg i t k k 1 Jk r gd t 3 两个相邻时刻的相对旋转是服从高斯分布的 可以证明在积分后 i 时刻和 j 时刻的相对旋转也是近似服从高 斯的 即 Rij RijExp ij 下面进行推导并求出 ij的递推公式 设初始时刻 Rii I3 3 ii 03 3 Ri i 1 Rii Ri i 1Exp Ji r gd i t i i 1 Ji r gd i t Ri i 2 Ri i 1 Ri 1 i 2 Ri i 1Exp i i 1 Ri 1 i 2Exp Ji 1 r gd i 1 t 5 Ri i 1 Ri 1 i 2Exp RT i 1 i 2 i i 1 Exp J i 1 r gd i 1 t 4 Ri i 2Exp RT i 1 i 2 i i 1 J i 1 r gd i 1 t i i 2 RT i 1 i 2 i i 1 Ji 1r gd i 1 t Ri i 3 Ri i 2 Ri 2 i 3 Ri i 2Exp i i 2 Ri 2 i 3Exp Ji 2 r gd i 2 t 5 Ri i 2 Ri 2 i 3Exp RT i 2 i 3 i i 2 Exp J i 2 r gd i 2 t 4 Ri i 3Exp RT i 2 i 3 i i 2 J i 2 r gd i 2 t i i 3 RT i 2 i 3 i i 2 Ji 2r gd i 2 t 因此 i k 1 RT k k 1 i k J k r gd k t velocity 对于速度而言 其高斯分布为 vk k 1 vk k 1 vk k 1 根据公式 10 中的 vi k 1 vi k Ri k ak ba i ad k t 进行推导 假设初始时刻 vi k vi k vi k vi k 1 vi k Ri k ak ba i ad k t vi k vi k Ri kExp i k ak ba i ad k t 1 vi k vi k Ri k I3 3 i k ak ba i ad k t vi k Ri k ak ba i t vi k Ri k ak ba i i k t Ri k ad k t vi k 1 vi k Ri k ak ba i i k t Ri k ad k t 因此 vi k 1 vi k Ri k ak ba i i k t Ri k ad k t Position 对于位移而言 其高斯分布为 pk k 1 pk k 1 pk k 1 根据公式 10 中的 pi k 1 pi k vi k t 1 2 Ri k ak ba i ad k t2进行推导 假设 pi k pi k pi k pi k 1 pi k vi k t 1 2 Ri k ak ba i ad k t2 pi k pi k vi k vi k t 1 2 Ri kExp i k ak ba i ad k t2 1 pi k vi k t pi k vi k t 1 2 Ri k I3 3 i k ak ba i ad k t2 pi k vi k t 1 2 Ri k ak ba i t 2 pi k vi k t 1 2 Ri k ak ba i i k t 2 1 2 Ri k ad k t2 pi k 1 pi k vi k t 1 2 Ri k ak ba i i k t 2 1 2 Ri k ad k t2 因此 pi k 1 pi k vi k t 1 2 Ri k ak ba i i k t2 1 2 Ri k ad k t2 综上 可以得到 ij 的递推计算公式 4 i k 1 RT k k 1 i k J k r gd k t vi k 1 vi k Ri k ak ba i i k t Ri k ad k t pi k 1 pi k vi k t 1 2 Ri k ak ba i i k t 2 1 2 Ri k ad k t2 13 写成矩阵的形式 i k 1 pi k 1 vi k 1 RT k k 1 03 303 3 1 2 Ri k ak ba i t2 I3 3I3 3 t Ri k ak ba i t 03 3I3 3 9 9 i k pi k vi k 03 3 1 2 Ri k t2 Ri k t 9 3 ad k Jk r t 03 3 03 3 9 3 gd k i k 1 Ak i k 1 Bk ad k Ck gd k 3 2Bias Correction via First Order Updates 在前面的推导中 我们假定在时刻 i 和 j 之间的偏置是相同的 然而在优化过程中偏置会得到修正 一种简 单的方式是将更新后的偏置代入上面的方程中 再对时刻 i 和 j 之间的测量进行积分 这样显然不是高效的 设 b b b 其中 b 为上一次的估计 b 为微小的增量更新 对公式 9 在 b 处进行一阶泰勒展开 Rij bg i j 1 k i Exp wk bg i bg i t Rij bg i Exp Rij bg bg i vij bg i ba i j 1 k i Rik bg i ak ba i ba i t vij bg i ba i vij bg bg i vij ba ba i pij bg i ba i j 1 k i vik bg i ba i t 1 2 Rik bg i ak ba i ba i t pij bg i ba i pij bg bg i pij ba ba i 14 因此当偏置更新后 我们只需要更新 preintegration measurements 中由于微小增量 b 带来的更新 上式与 式 7 的形式相同 仿照式 10 可以写出其迭代形式 Ri k 1 bg i Ri k bg i Exp wk bg i bg i t vi k 1 bg i ba i vi k bg i ba i Ri k bg i ak ba i ba i t pi k 1 bg i ba i pi k bg i ba i vi k bg i ba i t 1 2 Ri k bg i ak ba i ba i t 2 15 按照 3 1 节的推导方法可以写出 Rij bg vij bg vij ba pij bg pij ba 的递推形式 由于式 14 与式 8 的定义 不同 因此递推公式在符号上和式 13 不太一样 但形式是相同的 Ri k 1 bg RT k k 1 bg i Ri k bg Jk r t vi k 1 bg vi k bg Ri k bg i ak ba i Ri k bg t vi k 1 ba vi k ba Ri k bg i t pi k 1 bg pi k bg vi k bg t 1 2 Ri k bg i ak ba i Ri k bg t2 pi k 1 ba pi k ba vi k ba t 1 2 Ri k bg i t 2 16 5 令 Ha k Ri k ba T pi k ba T vi k ba T T 9 3 H g k Ri k bg T pi k bg T vi k bg T T 9 3 Hk Ha k Hg k 9 6 结果 整理可以得到其矩阵形式 Ha k 1 AkH a k Bk Hg k 1 AkHg k Ck 17 4IMU Factors Residual Errors and Jacobians 由式子 8 给定的 preintegration measurements 模型中 测量噪声是零均值高斯分布的 在一阶近似时 这