IMU Preintegration On Manifold(1)

1 SO(3) Group

关于 SO(3) 的介绍略过，这里只列出几个近似的公式：
Exp(δθ)≈I+[θ]×(1)
Exp(θ+δθ)≈Exp(θ)Exp(Jr(θ)δθ)(2)
Exp(θ)Exp(δθ)≈Exp(θ+J−1r(θ)δθ)(3)

Exp(δϕ)Exp(δθ)≈Exp(δϕ+J−1r(δϕ)δθ)≈Exp(δϕ+δθ)(4)

以及Adjoint表示：

Exp(θ)R=RExp(RTθ)(5)

2 IMU Preintegration measurements

2.1 Integration measurements

给定初值，在i和j时刻对imu的角速度和加速度进行积分，可以计算j时刻相对于i时刻的姿态：
Rjvjpj=Ri∏k=ij−1Exp((w̃ k−bgi−ηgdk)Δt)=vi+∑k=ij−1(g+Rk(ã k−bai−ηadk))Δt=pi+∑k=ij−1vkΔt+12∑k=ij−1(g+Rk(ã k−bai−ηadk))Δt2(6)

2.2 Preintegration measurements

在preintegration理论中需要将初值 (Ri,vi,pi) 和常数项（包含重力 g 的项）分离出来：
ΔRijΔvijΔpij=RTiRj=∏k=ij−1Exp((w̃ k−bgi−ηgdk)Δt)=RTi(vj−vi−gΔt(j−i))=∑k=ij−1ΔRik(ã k−bai−ηadk)Δt=RTi(pj−pi−viΔt(j−i)−12gΔt2(j−i)2)=∑k=ij−1[ΔvikΔt+12ΔRik(ã k−bai−ηadk)Δt2](7)

ΔRij,Δvij,Δpij 即为preintegration measurements，即不考虑初值以及重力加速度项的相对测量。注意到这些项包含有噪声 η ，我们也需要将它们分离出来。在分离的过程中发现preintegration measurements是近似服从高斯分布的，即：
ΔR̃ ijΔṽ ijΔp̃ ij≈ΔRijExp(δϕij)≈Δvij+δvij≈Δpij+δpij(8)

其中 ΔR̃ ij,Δṽ ij,Δp̃ ij 为我们可以计算的测量值，不包含噪声 η 。
ΔR̃ ijΔṽ ijΔp̃ ij=∏k=ij−1Exp((w̃ k−bgi)Δt)=∑k=ij−1ΔR̃ ik(ã k−bai)Δt=∑k=ij−1[Δṽ ikΔt+12ΔR̃ ik(ã k−bai)Δt2](9)

定义 ηΔij=[δϕTij,δpTij,δvTij]T9×1≈(09×1,∑ij) 为noise preintegration vector，它们是和噪声 η 相关的项。这里不会对 ηΔij 进行求解，因为事实上我们仅需要其递推形式。

2.3 Iterative preintegration measurements

首先给出包含噪声的递推公式：
ΔRi,k+1Δvi,k+1Δpi,k+1=ΔRi,kExp((w̃ k−bgi−ηgdk)Δt)=Δvi,k+ΔRi,k(ã k−bai−ηadk)Δt=Δpi,k+Δvi,kΔt+12ΔRi,k(ã k−bai−ηadk)Δt2(10)

接着给出不含噪声的递推公式：
ΔR̃ i,k+1Δṽ i,k+1Δp̃ i,k+1=ΔR̃ i,kExp((w̃ k−bgi)Δt)=Δṽ i,k+ΔR̃ i,k(ã k−bai)Δt=Δp̃ i,k+Δṽ i,kΔt+12ΔR̃ i,k(ã k−bai)Δt2(11)

3 IMU Preintegration: Noise Propagation and Bias Updates

3.1 Iterative Noise Propagation

在前面提到noise preintegration vector ηΔij=[δϕTij,δpTij,δvTij]T≈(09×1,∑ij) ，这里将证明preintegration measurements近似服从高斯分布，并给出 ηΔij 的递推计算结果。

Rotation
根据 SO(3) 中不确定性的定义，有 ΔR̃ k,k+1=ΔRk,k+1Exp(δϕk,k+1) 。
ΔRk,k+1 表示包含bias和noise两个相邻时刻的相对旋转， ΔR̃ k,k+1 表示不包含noise两个相邻时刻的相对旋转。

ΔRk,k+1=Exp((wk~−bgi−ηgdk)Δt)≈(2)Exp((wk~−bgi)Δt)Exp(−Jr((w̃ k−bgi)Δt)ηgdkΔt)=ΔR̃ k,k+1Exp(−JkrηgdΔt)=ΔR̃ k,k+1Exp(−ϕk,k+1)(12)

其中 ΔR̃ k,k+1=Exp((wk~−bgi)Δt) ， Jkr=Jr((wk~−bgi)Δt) ， ϕk,k+1=JkrηgdΔt 。

两个相邻时刻的相对旋转是服从高斯分布的，可以证明在积分后i时刻和j时刻的相对旋转也是近似服从高斯的，即 ΔR̃ ij≈ΔRijExp(δϕij) ，下面进行推导并求出 δϕij 的递推公式：

设初始时刻 ΔRii=I3×3,δϕii=03×3

ΔRi,i+1≈ΔRiiΔR̃ i,i+1Exp(−JirηgdiΔt)
⟹δϕi,i+1=JirηgdiΔt

ΔRi,i+2=ΔRi,i+1ΔRi+1,i+2≈ΔR̃ i,i+1Exp(−δϕi,i+1)ΔR̃ i+1,i+2Exp(−Ji+1rηgdi+1Δt)=(5)ΔR̃ i,i+1ΔR̃ i+1,i+2Exp(−ΔR̃ Ti+1,i+2δϕi,i+1)Exp(−Ji+1rηgdi+1Δt)≈(4)ΔR̃ i,i+2Exp(−ΔR̃ Ti+1,i+2δϕi,i+1−Ji+1rηgdi+1Δt)
⟹ϕi,i+2=ΔR̃ Ti+1,i+2δϕi,i+1+Ji+1rηgdi+1Δt

ΔRi,i+3=ΔRi,i+2ΔRi+2,i+3≈ΔR̃ i,i+2Exp(−ϕi,i+2)ΔR̃ i+2,i+3Exp(−Ji+2rηgdi+2Δt)=(5)ΔR̃ i,i+2ΔR̃ i+2,i+3Exp(−ΔR̃ Ti+2,i+3ϕi,i+2)Exp(−Ji+2rηgdi+2Δt)≈(4)ΔR̃ i,i+3Exp(−ΔR̃ Ti+2,i+3ϕi,i+2−Ji+2rηgdi+2Δt)
⟹ϕi,i+3=ΔR̃ Ti+2,i+3ϕi,i+2+Ji+2rηgdi+2Δt

因此 δϕi,k+1=ΔR̃ Tk,k+1δϕi,k+JkrηgdkΔt 。

velocity

对于速度而言，其高斯分布为 Δṽ k,k+1=Δvk,k+1+δvk,k+1 。

根据公式(10) (10) 中的\Delta v_{i,k+1}=\Delta v_{i,k}+\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt Δvi,k+1=Δvi,k+ΔRi,k(ã k−bai−ηadk)Δt 进行推导。

假设初始时刻\Delta\tilde{v}_{i,k}=\Delta v_{i,k}+\delta v_{i,k} Δṽ i,k=Δvi,k+δvi,k 。

\begin{aligned}\Delta v_{i,k+1} & =\Delta v_{i,k}+\Delta R_{i,k}(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\ & =\Delta\tilde{v}_{i,k}-\delta v_{i,k}+\Delta\tilde{R}_{i,k}\Exp(-\delta\phi_{i,k})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\ & \stackrel{(1)}{\approx}\Delta\tilde{v}_{i,k}-\delta v_{i,k}+\Delta\tilde{R}_{i,k}(\textbf{I}_{3\times3}-\Skew{\delta\phi_{i,k}})(\tilde{a}_{k}-b_{i}^{a}-\eta_{k}^{ad})\dt\\ & \approx\Delta\tilde{v}_{i,k}+\Delta\tilde{R}_{i,k}(\tilde{a}_{k}-b_{i}^{a})\dt-(\delta v_{i,k}-\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt)\\ & =\Delta\tilde{v}_{i,k+1}-(\delta v_{i,k}-\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\delta\phi_{i,k}\dt+\Delta\tilde{R}_{i,k}\eta_{k}^{ad}\dt) \end{aligned} Δvi,k+1=Δvi,k+ΔRi,k(ã k−bai−ηadk)Δt=Δṽ i,k−δvi,k+ΔR̃ i,kExp(−δϕi,k)(ã k−bai−ηadk)Δt≈(1)Δṽ i,k−δvi,k+ΔR̃ i,k(I3×3−[δϕi,k]×)(ã k−bai−ηadk)Δt≈Δṽ i,k+ΔR̃ i,k(ã k−bai)Δt−(δvi,k−ΔR̃ i,k[ã k−bai]×δϕi,kΔt+ΔR̃ i,kηadkΔt)=Δṽ i,k+1−(δvi,k−ΔR̃ i,k[ã k−bai]×δϕi,kΔt+ΔR̃ i,kηadkΔt)

因此 δvi,k+1=δvi,k−ΔR̃ Ti,k[ã k−bai]×δϕi,kΔt+ΔR̃ Ti,kηadkΔt 。

Position

对于位移而言，其高斯分布为 Δp̃ k,k+1=Δpk,k+1+δpk,k+1 。

根据公式 (10) 中的 Δpi,k+1=Δpi,k+Δvi,kΔt+12ΔRi,k(ã k−bai−ηadk)Δt2 进行推导。

假设\Delta\tilde{p}_{i,k}=\Delta p_{i,k}+\delta p_{i,k} Δp̃ i,k=Δpi,k+δpi,k

Δpi,k+1=Δpi,k+Δvi,kΔt+12ΔRi,k(ã k−bai−ηadk)Δt2=Δp̃ i,k−δpi,k+(Δṽ i,k−δvi,k)Δt+12ΔR̃ i,kExp(−δϕi,k)(ã k−bai−ηadk)Δt2≈(1)Δp̃ i,k+Δṽ i,kΔt−δpi,k−δvi,kΔt+12ΔR̃ i,k(I3×3−[δϕi,k]×)(ã k−bai−ηadk)Δt2≈Δp̃ i,k+Δṽ i,kΔt+12ΔR̃ i,k(ã k−bai)Δt2−δpi,k−δvi,kΔt+12ΔR̃ i,k[ã k−bai]×δϕi,kΔt2−12ΔR̃ i,kηadkΔt2≈Δp̃ i,k+1−(δpi,k+δvi,kΔt−12ΔR̃ i,k[ã k−bai]×δϕi,kΔt2+12ΔR̃ i,kηadkΔt2)

因此 δpi,k+1=δpi,k+δvi,kΔt−12ΔR̃ i,k[ã k−bai]×δϕi,kΔt2+12ΔR̃ i,kηadkΔt2 。

综上，可以得到 ηΔij 的递推计算公式：

δϕi,k+1δvi,k+1δpi,k+1=ΔR̃ Tk,k+1δϕi,k+JkrηgdkΔt=δvi,k−ΔR̃ Ti,k[ã k−bai]×δϕi,kΔt+ΔR̃ Ti,kηadkΔt=δpi,k+δvi,kΔt−12ΔR̃ i,k[ã k−bai]×δϕi,kΔt2+12ΔR̃ i,kηadkΔt2(13)

写成矩阵的形式：
\left[\begin{array}{c} \delta\phi_{i,k+1}\\ \delta p_{i,k+1}\\ \delta v_{i,k+1} \end{array}\right]=\left[\begin{array}{ccc} \Delta\tilde{R}_{k,k+1}^{T} & \MatZ 3 & \MatZ 3\\ -\frac{1}{2}\Delta\tilde{R}_{i,k}\Skew{\tilde{a}_{k}-b_{i}^{a}}\dt^{2} & \MatI 3 & \MatI 3\dt\\ -\Delta\tilde{R}_{i,k}^{T}\Skew{\tilde{a}_{k}-b_{i}^{a}}\dt & \MatZ 3 & \MatI 3 \end{array}\right]_{9\times9}\left[\begin{array}{c} \delta\phi_{i,k}\\ \delta p_{i,k}\\ \delta v_{i,k} \end{array}\right]+\left[\begin{array}{c} \MatZ 3\\ \frac{1}{2}\Delta\tilde{R}_{i,k}\dt^{2}\\ \Delta\tilde{R}_{i,k}^{T}\dt \end{array}\right]_{9\times3}\eta_{k}^{ad}+\left[\begin{array}{c} J_{r}^{k}\Delta t\\ \MatZ 3\\ \MatZ 3 \end{array}\right]_{9\times3}\eta_{k}^{gd}

⎡⎣⎢⎢⎢δϕi,k+1δpi,k+1δvi,k+1⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢ΔR̃ Tk,k+1−12ΔR̃ i,k[ã k−bai]×Δt2−ΔR̃ Ti,k[ã k−bai]×Δt03×3I3×303×303×3I3×3ΔtI3×3⎤⎦⎥⎥⎥⎥9×9⎡⎣⎢⎢⎢δϕi,kδpi,kδvi,k⎤⎦⎥⎥⎥+⎡⎣⎢⎢⎢⎢03×312ΔR̃ i,kΔt2ΔR̃ Ti,kΔt⎤⎦⎥⎥⎥⎥9×3ηadk+⎡⎣⎢⎢⎢JkrΔt03×303×3⎤⎦⎥⎥⎥9×3ηgdk

\eta_{i,k+1}^{\Delta}=A_{k}\eta_{i,k+1}^{\Delta}+B_{k}\eta_{k}^{ad}+C_{k}\eta_{k}^{gd} ηΔi,k+1=AkηΔi,k+1+Bkηadk+Ckηgdk

3.2 Bias Correction via First-Order Updates

在前面的推导中，我们假定在时刻i和j之间的偏置是相同的，然而在优化过程中偏置会得到修正。一种简单的方式是将更新后的偏置代入上面的方程中，再对时刻i和j之间的测量进行积分，这样显然不是高效的。设\hat{b}\leftarrow\bar{b}+\delta b b̂ ←b¯+δb ，其中\bar{b} b¯ 为上一次的估计，\delta b δb 为微小的增量更新。对公式(9) (9) 在\bar{b} b¯ 处进行一阶泰勒展开：

\begin{aligned}\Delta\tilde{R}_{ij}(\hat{b}_{i}^{g}) & =\prod_{k=i}^{j-1}\textrm{Exp}((\tilde{w}_{k}-\bar{b}_{i}^{g}-\delta b_{i}^{g})\dt)=\Delta\tilde{R}_{ij}(\bar{b}_{i}^{g})\Exp(\frac{\partial\Delta\bar{R}_{ij}}{\partial b^{g}}\delta b_{i}^{g})\\ \Delta\tilde{v}_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\sum_{k=i}^{j-1}\Delta\tilde{R}{}_{ik}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt=\Delta\tilde{v}_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{g}}\delta b_{i}^{g}+\frac{\partial\Delta\bar{v}_{ij}}{\partial b^{a}}\delta b_{i}^{a}\\ \Delta\tilde{p}_{ij}(\hat{b}_{i}^{g},\hat{b}_{i}^{a}) & =\sum_{k=i}^{j-1}[\Delta\tilde{v}{}_{ik}(\hat{b}_{i}^{g},\hat{b}_{i}^{a})\dt+\frac{1}{2}\Delta\tilde{R}{}_{ik}(\hat{b}_{i}^{g})(\tilde{a}_{k}-\bar{b}_{i}^{a}-\delta b_{i}^{a})\dt]=\Delta\tilde{p}_{ij}(\bar{b}_{i}^{g},\bar{b}_{i}^{a})+\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{g}}\delta b_{i}^{g}+\frac{\partial\Delta\bar{p}_{ij}}{\partial b^{a}}\delta b_{i}^{a} \end{aligned} \tag{14} ΔR̃ ij(b̂ gi)Δṽ ij(b̂ gi,b̂ ai)Δp̃