# ORB_SLAM3中IMU预积分过程原理分析

32 篇文章 22 订阅

## ORB_SLAM3中IMU预积分过程原理分析

#### 1. 特殊正交群SO(3)的一些性质

e x p ( ϕ ^ ) = I + s i n ( ∥ ϕ ∥ ) ∥ ϕ ∥ ϕ ^ + 1 − c o s ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 ( ϕ ^ ) 2 (1) exp(\phi^{\hat{ }}) = \mathbf{I} + \frac{sin(\left\|\phi \right\|)}{\left\|\phi \right\|}\phi^{\hat{ }} + \frac{1-cos(\left\|\phi \right\|)}{\left\|\phi \right\|^{2}}(\phi^{\hat{ }})^{2} \tag{1}

e x p ( ϕ ^ ) ≈ I + ϕ ^ (2) exp(\phi^{\hat{ }}) \approx \mathbf{I} + \phi^{\hat{ }} \tag{2}

l o g ( R ) = φ ⋅ ( R − R T ) 2 s i n ( φ ) 其 中 φ = c o s − 1 ( t r ( R ) − 1 2 ) (3) log(\mathbf{R}) = \frac{\varphi\cdot(\mathbf{R}-\mathbf{R}^{T})}{2sin(\varphi)}其中\varphi=cos^{-1}(\frac{tr(\mathbf{R})-1}{2}) \tag{3}
BCH公式与近似形式(14讲72页):
E x p ( ϕ + δ ϕ ) ≈ E x p ( ϕ ) E x p ( J r ( ϕ ) δ ϕ ) (4) Exp(\phi + \delta\phi) \approx Exp(\phi)Exp(\mathbf{J}_{r}(\phi)\delta\phi) \tag{4}

J r ( ϕ ) = I − 1 − c o s ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 + ∥ ϕ ∥ − s i n ( ∥ ϕ ∥ ) ∥ ϕ 3 ∥ ( ϕ ^ ) 2 (5) \mathbf{J}_{r}(\phi) = \mathbf{I} - \frac{1-cos(\left\|\phi \right\|)}{\left\|\phi \right\|^{2}} + \frac{\left\|\phi \right\|-sin(\left\|\phi \right\|)}{\left\|\phi ^{3}\right\|} (\phi^{\hat{ }})^{2} \tag{5}

E x p ( ϕ ) R = R E x p ( R T ϕ ) (6) Exp(\phi)\mathbf{R} = \mathbf{R}Exp(\mathbf{R}^{T}\phi) \tag{6}

#### 2. IMU测量模型和运动模型

B ω ~ W B ( t ) = B ω W B ( t ) + b g ( t ) + η g ( t ) B a ~ ( t ) = R W B T ( W a ( t ) − W g ) + b a ( t ) + η a ( t ) (7) \begin{aligned} {}_{_B}\widetilde{\boldsymbol{\omega}}_{_{WB}}(t) &= {}_{_B}\boldsymbol{\omega}_{_{WB}}(t) + \mathbf{b}^{g}(t) + \boldsymbol{\eta}^{g}(t) \\ {}_{_B}\widetilde{\mathbf{a}}(t) &= \mathbf{R}_{_{WB}}^{T}({}_{_W}\mathbf{a}(t)-{}_{_W}\mathbf{g}) + \mathbf{b}^{a}(t) + \boldsymbol{\eta}^{a}(t) \tag{7} \end{aligned}

R ˙ W B = R W B ⋅ B ω W B ^ v ˙ W = W a p ˙ W = W v (8) \begin{aligned} \dot{\mathbf{R}}_{_{WB}} &= \mathbf{R}_{_{WB}} \cdot{}_{_B}\boldsymbol{\omega}_{_{WB}}\hat{}\\ \dot{\mathbf{v}}_{_W} &= {}_{_W}\mathbf{a} \\ \dot{\mathbf{p}}_{_W} &= {}_{_W}\mathbf{v} \tag{8} \end{aligned}

R W B ( t + Δ t ) = R W B E x p ∫ t t + Δ t B ω W B ( τ ) d τ W v ( t + Δ t ) = W v ( t ) + ∫ t t + Δ t W a ( τ ) d τ W p ( t + Δ t ) = W p ( t ) + ∫ t t + Δ t W v ( τ ) d τ + ∬ t t + Δ t W a ( τ ) d τ 2 (9) \begin{aligned} \mathbf{R}_{_{WB}}(t+\Delta t) &= \mathbf{R}_{_{WB}} Exp{ \int_{t}^{t+\Delta{t}} {}_{_{B}}\boldsymbol{\omega}_{_{WB}}(\tau) d\tau } \\ {}_{_{W}}\mathbf{v}(t+\Delta t) &= {}_{_{W}}\mathbf{v}(t) + \int_{t}^{t+\Delta t} {}_{_W}\mathbf{a}(\tau)d\tau \\ {}_{_{W}}\mathbf{p}(t+\Delta t) &= {}_{_{W}}\mathbf{p}(t) + \int_{t}^{t+\Delta t} {}_{_W}\mathbf{v}(\tau)d\tau + \iint_{t}^{t+\Delta t} {}_{_W}\mathbf{a}(\tau)d\tau^{2} \tag{9} \end{aligned}

R W B ( t + Δ t ) = R W B E x p ( B ω W B ( t ) Δ t ) W v ( t + Δ t ) = W v ( t ) + W a ( t ) Δ t W p ( t + Δ t ) = W p ( t ) + W v ( t ) Δ t + 1 2 W a ( t ) Δ t 2 (10) \begin{aligned} \mathbf{R}_{_{WB}}(t+\Delta t) &= \mathbf{R}_{_{WB}} Exp({}_{_B}\boldsymbol{\omega}_{_{WB}}(t)\Delta t )\\ {}_{_{W}}\mathbf{v}(t+\Delta t) &= {}_{_{W}}\mathbf{v}(t) + {}_{_W}\mathbf{a}(t)\Delta t\\ {}_{_{W}}\mathbf{p}(t+\Delta t) &= {}_{_{W}}\mathbf{p}(t) + {}_{_W}\mathbf{v}(t)\Delta t + \frac{1}{2}{}_{_W}\mathbf{a}(t){\Delta t}^{2} \tag{10} \end{aligned}

R ( t + Δ t ) = R ( t ) E x p ( ( ω ~ ( t ) − b g ( t ) − η g d ( t ) ) Δ t ) v ( t + Δ t ) = v ( t ) + g Δ t + R ( t ) ( a ~ ( t ) − b a ( t ) − η a d ( t ) ) Δ t p ( t + Δ t ) = p ( t ) + v ( t ) Δ t + 1 2 g Δ t 2 + 1 2 R ( t ) ( a ~ ( t ) − b a ( t ) − η a d ( t ) ) Δ t 2 (11) \begin{aligned} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t)Exp((\widetilde{\boldsymbol{\omega}}(t)-\mathbf{b}^{g}(t)-\boldsymbol{\eta}^{gd}(t))\Delta t) \\ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g}\Delta t + \mathbf{R}(t) (\widetilde{\mathbf{a}}(t) - \mathbf{b}^{a}(t) - \boldsymbol{\eta}^{ad}(t))\Delta t \\ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t)\Delta t + \frac{1}{2}\mathbf{g}\Delta t^{2} + \frac{1}{2}\mathbf{R}(t) (\widetilde{\mathbf{a}}(t) - \mathbf{b}^{a}(t) - \boldsymbol{\eta}^{ad}(t))\Delta t^{2} \tag{11} \end{aligned}

#### 3. IMU预积分

R j = R i ∏ k = i j − 1 E x p ( ( w ~ k − b k g − η k g d ) Δ t ) v j = v i + g Δ t i j + ∑ k = i j − 1 R k ( a ~ k − b k a − η k a d ) Δ t p j = p i + ∑ k = i j − 1 [ v k Δ t + 1 2 g Δ t 2 + 1 2 R k ( a ~ k − b k a − η k a d ) Δ t 2 ] (12) \begin{aligned} \mathbf{R}_{j} &= \mathbf{R}_{i}\prod_{k=i}^{j-1}Exp((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{gd})\Delta t) \\ \mathbf{v}_{j} &= \mathbf{v}_{i} + \mathbf{g}\Delta t_{ij} + \sum_{k=i}^{j-1}\mathbf{R}_{k}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t \\ \mathbf{p}_{j} &= \mathbf{p}_{i} + \sum_{k=i}^{j-1}[\mathbf{v}_{k}\Delta t + \frac{1}{2}\mathbf{g}\Delta t^{2} + \frac{1}{2}\mathbf{R}_{k}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t^{2}] \tag{12} \end{aligned}

Δ R i j = R i T R j = ∏ k = i j − 1 E x p ( ( w ~ k − b k g − η k g d ) Δ t ) Δ v i j = R i T ( v j − v i − g Δ t i j ) = ∑ k = i j − 1 R i k ( a ~ k − b k a − η k a d ) Δ t Δ p i j = R i T ( p j − p i − 1 2 g Δ t i j 2 ) = ∑ k = i j − 1 [ v i k Δ t + 1 2 R i k ( a ~ k − b k a − η k a d ) Δ t 2 ] (13) \begin{aligned} \Delta \mathbf{R}_{ij} &= \mathbf{R}_{i}^{T}\mathbf{R}_{j}=\prod_{k=i}^{j-1}Exp((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{gd})\Delta t)\\ \Delta \mathbf{v}_{ij} &= \mathbf{R}_{i}^{T}(\mathbf{v}_{j} - \mathbf{v}_{i} - \mathbf{g}\Delta t_{ij}) = \sum_{k=i}^{j-1}\mathbf{R}_{ik}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t \\ \Delta \mathbf{p}_{ij} &= \mathbf{R}_{i}^{T}(\mathbf{p}_{j}-\mathbf{p}_{i}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^{2}) = \sum_{k=i}^{j-1}[\mathbf{v}_{ik}\Delta t + \frac{1}{2}\mathbf{R}_{ik}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t^{2}] \tag{13} \end{aligned}

##### 3.1.1 姿态测量模型

Δ R i j = R i T R j = ∏ k = i j − 1 E x p ( ( w ~ k − b k g − η k g d ) Δ t ) ≃ ( 4 ) ∏ k = i j − 1 E x p ( ( w ~ k − b k g ) Δ t ) E x p ( − J r k ( ( w ~ k − b k g ) Δ t ) η k g d Δ t ) = ( 6 ) Δ R ~ i j ∏ k = i j − 1 E x p ( − Δ R ~ k + 1 , j T J r k ( ( w ~ k − b k g ) Δ t ) η k g d Δ t ) = Δ R ~ i j E x p ( − δ ϕ i j ) (14) \begin{aligned} \Delta \mathbf{R}_{ij} &= \mathbf{R}_{i}^{T}\mathbf{R}_{j}\\ &=\prod_{k=i}^{j-1}Exp((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{gd})\Delta t)\\ &\overset{(4)}\simeq \prod_{k=i}^{j-1}Exp((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g})\Delta t) Exp(-\mathbf{J}_{r}^{k}((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g})\Delta t)\boldsymbol{\eta}_{k}^{gd}\Delta t) \\ &\overset{(6)}=\Delta\widetilde{\mathbf{R}}_{ij}\prod_{k=i}^{j-1}Exp(-\Delta\widetilde{\mathbf{R}}_{k+1,j}^{T}\mathbf{J}_{r}^{k}((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g})\Delta t) \boldsymbol{\eta}_{k}^{gd}\Delta t) \\ &= \Delta\widetilde{\mathbf{R}}_{ij}Exp(-\delta\phi_{ij}) \end{aligned} \tag{14}

Δ R ~ i j = ∏ k = i j − 1 E x p ( ( w ~ k − b k g ) Δ t ) \Delta\widetilde{\mathbf{R}}_{ij} = \prod_{k=i}^{j-1}Exp((\widetilde{\boldsymbol{w}}_{k} - \mathbf{b}_{k}^{g})\Delta t)

Δ R ~ i j = R i T R j E x p ( δ ϕ i j ) (15) \Delta\widetilde{\mathbf{R}}_{ij} = \mathbf{R}_{i}^{T}\mathbf{R}_{j}Exp(\delta\phi_{ij}) \tag{15}

##### 3.1.2 速度测量模型

Δ v i j = R i T ( v j − v i − g Δ t i j ) = ∑ k = i j − 1 R i k ( a ~ k − b k a − η k a d ) Δ t ≃ ( 2 , 14 ) ∑ k = i j − 1 Δ R ~ i k ( I − δ ϕ i k ^ ) ( a ~ k − b i a ) Δ t − Δ R ~ i k η k a d Δ t = Δ v ~ i j + ∑ k = i j − 1 [ Δ R ~ i k ( a ~ k − b i a ) ^ δ ϕ i k Δ t − Δ R ~ i k η k a d Δ t ] = Δ v ~ i j − δ v i j (16) \begin{aligned} \Delta \mathbf{v}_{ij} &=\mathbf{R}_{i}^{T}(\mathbf{v}_{j} - \mathbf{v}_{i} - \mathbf{g}\Delta t_{ij})\\ &= \sum_{k=i}^{j-1}\mathbf{R}_{ik}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t \\ &\overset{(2,14)}\simeq \sum_{k=i}^{j-1}\Delta\widetilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\phi_{ik}\hat{})(\widetilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a})\Delta t - \Delta\widetilde{\mathbf{R}}_{ik}\boldsymbol{\eta}_{k}^{ad}\Delta t \\ &= \Delta\widetilde{\mathbf{v}}_{ij} + \sum_{k=i}^{j-1}[\Delta\widetilde{\mathbf{R}}_{ik}(\widetilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a})\hat{}\delta\phi_{ik}\Delta t - \Delta\widetilde{\mathbf{R}}_{ik}\boldsymbol{\eta}_{k}^{ad}\Delta t] \\ &= \Delta\widetilde{\mathbf{v}}_{ij} - \delta\mathbf{v}_{ij} \tag{16} \end{aligned}

Δ v ~ i j = ∑ k = i j − 1 Δ R ~ i k ( a ~ k − b i a ) Δ t \Delta\widetilde{\mathbf{v}}_{ij} = \sum_{k=i}^{j-1}\Delta\widetilde{\mathbf{R}}_{ik}(\widetilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a})\Delta t

Δ v ~ i j = R i T ( v j − v i − g Δ t i j ) + δ v i j (17) \Delta\widetilde{\mathbf{v}}_{ij} = \mathbf{R}_{i}^{T}(\mathbf{v}_{j} - \mathbf{v}_{i} - \mathbf{g}\Delta t_{ij}) + \delta\mathbf{v}_{ij} \tag{17}

##### 3.1.3 位置测量模型

Δ p i j = R i T ( p j − p i − 1 2 g Δ t i j 2 ) = ∑ k = i j − 1 [ v i k Δ t + 1 2 R i k ( a ~ k − b k a − η k a d ) Δ t 2 ] ≃ 2 , 14 , 16 ∑ k = i j − 1 [ ( Δ v ~ i k − δ v i k ) Δ t + 1 2 Δ R ~ i k ( I − δ ϕ i k ^ ) ( a ~ k − b i a ) Δ t 2 − 1 2 Δ R ~ i k η k a d Δ t 2 ] = Δ p ~ i j + ∑ k = i j − 1 [ − δ v i k Δ t + 1 2 Δ R ~ i k ( a ~ k − b i a ) ^ δ ϕ i k Δ t 2 − 1 2 Δ R ~ i k η k a d Δ t 2 ] ] = Δ p ~ i j − δ p i j (18) \begin{aligned} \Delta \mathbf{p}_{ij} &= \mathbf{R}_{i}^{T}(\mathbf{p}_{j}-\mathbf{p}_{i}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^{2})\\ &= \sum_{k=i}^{j-1}[\mathbf{v}_{ik}\Delta t + \frac{1}{2}\mathbf{R}_{ik}(\widetilde{\mathbf{a}}_{k} - \mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{ad})\Delta t^{2}]\\ &\overset{2,14,16}\simeq \sum_{k=i}^{j-1}[(\Delta\widetilde{\mathbf{v}}_{ik}-\delta\mathbf{v}_{ik})\Delta t + \frac{1}{2}\Delta\widetilde{\mathbf{R}}_{ik}(\mathbf{I}-\delta\phi_{ik}\hat{})(\widetilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a})\Delta t^{2} - \frac{1}{2}\Delta\widetilde{\mathbf{R}}_{ik}\boldsymbol{\eta}_{k}^{ad}\Delta t^{2}]\\ &= \Delta\widetilde{\mathbf{p}}_{ij} + \sum_{k=i}^{j-1}[-\delta\mathbf{v}_{ik}\Delta t + \frac{1}{2}\Delta\widetilde{\mathbf{R}}_{ik}(\widetilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a})\hat{}\delta\phi_{ik}\Delta t^{2}- \frac{1}{2}\Delta\widetilde{\mathbf{R}}_{ik}\boldsymbol{\eta}_{k}^{ad}\Delta t^{2}]]\\ &= \Delta\widetilde{\mathbf{p}}_{ij} - \delta\mathbf{p}_{ij} \tag{18} \end{aligned}

Δ p ~ i j = R i T ( p j − p i − 1 2 g Δ t i j 2 ) + δ p i j (19) \Delta\widetilde{\mathbf{p}}_{ij} = \mathbf{R}_{i}^{T}(\mathbf{p}_{j}-\mathbf{p}_{i}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^{2}) + \delta\mathbf{p}_{ij} \tag{19}

##### 3.2 噪声传递

Δ x i k = [ δ ϕ i k , δ v i k , δ p i k ] \Delta\mathbf{x}_{ik}=[\delta\phi_{ik}, \delta\mathbf{v}_{ik},\delta\mathbf{p}_{ik}] η k d = [ η k g d , η k a d ] \boldsymbol{\eta}_{k}^{d}=[\boldsymbol{\eta}_{k}^{gd}, \boldsymbol{\eta}_{k}^{ad}] ，于是上述三个公式可写为:
Δ x i j = [ Δ R ~ j − 1 , j T 0 0 − Δ R ~ i , j − 1 T ( a ~ j − 1 − b i a ) ^ Δ t I 0 − 1 2 Δ R ~ i , j − 1 T ( a ~ j − 1 − b i a ) ^ Δ t 2 Δ t I ] Δ x i , j − 1 + [ J r j − 1 0 0 Δ R ~ i , j − 1 Δ t 0 1 2 Δ R ~ i , j − 1 Δ t 2 ] η j − 1 d = A j − 1 Δ x i , j − 1 + B j − 1 η j − 1 d (20) \begin{aligned} \Delta\mathbf{x}_{ij} = \begin{bmatrix} \Delta\widetilde{\mathbf{R}}_{j-1,j}^{T} & 0 & 0 \\ -\Delta\widetilde{\mathbf{R}}_{i,j-1}^{T}(\mathbf{\widetilde{\mathbf{a}}}_{j-1}-\mathbf{b}_{i}^{a})\hat{}\Delta t & I & 0 \\ -\frac{1}{2}\Delta\widetilde{\mathbf{R}}_{i,j-1}^{T}(\mathbf{\widetilde{\mathbf{a}}}_{j-1}-\mathbf{b}_{i}^{a})\hat{}\Delta t^{2} & \Delta t & I \end{bmatrix} \Delta\mathbf{x}_{i,j-1} + \begin{bmatrix} \mathbf{J}_{r}^{j-1} & 0\\ 0 & \Delta\widetilde{\mathbf{R}}_{i,j-1}\Delta t \\ 0 & \frac{1}{2}\Delta\widetilde{\mathbf{R}}_{i,j-1}\Delta t^{2} \end{bmatrix} \boldsymbol{\eta}_{j-1}^{d} \end{aligned} = A_{j-1}\Delta\mathbf{x}_{i,j-1}+B_{j-1}\boldsymbol{\eta}_{j-1}^{d} \tag{20}

∑ i j = A j − 1 ∑ i , j − 1 A j − 1 T + B j − 1 ∑ η B j − 1 T (21) \sum_{ij} = A_{j-1}\sum_{i,j-1}A_{j-1}^{T} + B_{j-1}\sum_{\eta}B_{j-1}^{T} \tag{21}

----------------------------------- 公式已敲吐血 😦 😦 😦，后面内容后续再补 --------------------------------

#### 参考

• 6
点赞
• 18
收藏
觉得还不错? 一键收藏
• 4
评论
12-12 795
10-20 3945
04-13 675
09-24 483
03-29 1571
04-09 535
09-16 4096

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。