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)坐标系
以下均省略上下标
矢量场和微分方程
我们用以下微分方程描述导航问题 X ˙ ( t ) = F ( t , X ) \dot X(t)=(t,X) X˙(t)=F(t,X)
- F F F ：时变矢量场
- X ˙ ( t ) \dot X(t) X˙(t): 一个三元组的切向量， X ˙ ( t ) = [ R ˙ ( t , X ) , P ˙ ( t , X ) , V ˙ ( t , X ) ] ∈ s 0 ( 3 ) × R 3 × R 3 \dot X(t)=[\dot{R}(t, X), \dot{P}(t, X), \dot{V}(t, X)] \in \mathfrak{s0}(3) \times \mathbb{R}^{3} \times \mathbb{R}^{3} X˙(t)=[R˙(t,X),P˙(t,X),V˙(t,X)]∈s0(3)×R3×R3
- 在 X X X 处由所有切向量组成的空间表示为 T X M T_XM TXM ，即 F ( t , X ) ∈ T X M F(t,X) \in T_XM F(t,X)∈TXM
- R ˙ = R [ ω ] X \dot R=R[\omega ]_X R˙=R[ω]X
X ˙ ( t ) = [ R ˙ ( X , t ) , V ( t ) , V ˙ ( X , t ) ] = [ R ( t ) [ ω b ( t ) ] × , V ( t ) , g + R ( t ) a b ( t ) ] \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] X˙(t)=[R˙(X,t),V(t),V˙(X,t)]=[R(t)[ωb(t)]×,V(t),g+R(t)ab(t)]
局部坐标
基于流形空间的优化基于局部坐标的概念。当初始位姿为 R 0 R_0 R0，定义一个局部映射 Φ R 0 ( θ ) \Phi _{R_0}(\theta) ΦR0(θ)从局部坐标 θ ∈ R 3 \theta\in \mathbb{R}^3 θ∈R3 映射到 R 0 R_0 R0 的邻域： Φ R 0 ( θ ) = R 0 exp ( [ θ ] × ) \Phi_{R_{0}}(\theta)=R_{0} \exp \left([\theta]_{\times}\right) ΦR0(θ)=R0exp([θ]×) 局部坐标系 θ \theta θ 和切向量是同构的，定义 θ = ω t \theta=\omega t θ=ωt,有 d Φ R 0 ( ω t ) d t ∣ t = 0 = d R 0 exp ( [ ω t ] × ) d t ∣ t = 0 = R 0 [ ω ] × \left.\frac{d \Phi_{R_{0}}(\omega t)}{d t}\right|_{t=0}=\left.\frac{d R_{0} \exp \left([\omega t]_{\times}\right)}{d t}\right|_{t=0}=R_{0}[\omega]_{\times} dtdΦR0(ωt)∣∣∣∣t=0=dtdR0exp([ωt]×)∣∣∣∣t=0=R0[ω]×
- 原文这里有一个笔误，反对称矩阵里面应该没有 t t t
因此，三维向量 ω \omega ω表示了在 S O ( 3 ) SO(3) SO(3)流形上的前进方向，但是仅在 R 0 R_0 R0附近的局部坐标系上有效。
类似的，可以定义在 S E ( 3 ) SE(3) SE(3)上的局部坐标 ξ = [ ω t , v t ] ∈ R 6 \xi=[\omega t, v t] \in \mathbb{R}^{6} ξ=[ωt,vt]∈R6与其对应的映射函数 Φ T 0 ( ξ ) = T 0 exp ξ ^ \Phi_{T_{0}}(\xi)=T_{0} \exp \hat{\xi} ΦT0(ξ)=T0expξ^
局部坐标系映射的导数
定义一个 3 × 3 3\times3 3×3的雅克比矩阵模拟增量 δ \delta δ 对局部坐标系的影响。 Φ R 0 ( θ + δ ) ≈ Φ R 0 ( θ ) exp ( [ H ( θ ) δ ] × ) = Φ Φ R 0 ( θ ) ( H ( θ ) δ ) \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) ΦR0(θ+δ)≈ΦR0(θ)exp([H(θ)δ]×)=ΦΦR0(θ)(H(θ)δ) 这个雅克比的计算仅仅依赖 θ \theta θ H ( θ ) = ∑ k = 0 ∞ ( − 1 ) k ( k + 1 ) ! [ θ ] × k H(\theta)=\sum_{k=0}^{\infty} \frac{(-1)^{k}}{(k+1) !}[\theta]_{\times}^{k} H(θ)=k=0∑∞(k+1)!(−1)k[θ] 标签： ar03ftb5230精密电阻传感器lr18xbn08lum