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 即为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−b