本文主要是介绍ORB_SLAM3_IMU预积分理论推导(更新)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
- ORB SLAM3系统初始化
- ORB SLAM3 构建Frame
- ORB_SLAM3 单目初始化
- ORB_SLAM3 双目匹配
- ORB_SLAM3_IMU预积分理论推导(预积分项)
- ORB_SLAM3_IMU预积分理论推导(噪声分析)
- ORB_SLAM3_IMU预积分理论推导(更新)
- ORB_SLAM3_IMU预积分理论推导(残差)
- ORB_SLAM3_优化方法 Pose优化
- ORB_SLAM3 闭环检测
预积分测量值更新
当bias不发生变化时
-
Δ R ~ i j − 1 → Δ R ~ i j \Delta \tilde{R} _{ij-1}\to \Delta \tilde{R}_{ij} ΔR~ij−1→ΔR~ij
Δ R ~ i j = ∏ k = i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ) = ∏ k = i j − 2 Exp ( ( ω ~ k − b i g ) Δ t ) ⋅ Exp ( ( ω ~ j − 1 − b i g ) Δ t ) = Δ R ~ i j − 1 ⋅ Δ R ~ j − 1 j \begin{array}{l} \Delta \tilde{\mathbf{R}}_{i j}=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ =\prod_{k=i}^{j-2} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right)\cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{j-1}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ =\Delta \tilde{\mathbf{R}}_{i j-1}\cdot\Delta \tilde{\mathbf{R}}_{j-1 j} \end{array} ΔR~ij=∏k=ij−1Exp((ω~k−big)Δt)=∏k=ij−2Exp((ω~k−big)Δt)⋅Exp((ω~j−1−big)Δt)=ΔR~ij−1⋅ΔR~j−1j -
Δ v ~ i j − 1 → Δ v ~ i j \Delta \tilde{v}_{ij-1} \to \Delta \tilde{v}_{ij} Δv~ij−1→Δv~ij
Δ v ~ i j ≜ ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] ≜ ∑ k = i j − 2 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] + Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ⋅ Δ t ≜ Δ v ~ i j − 1 + Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ⋅ Δ t \begin{array}{l} \Delta \tilde{\mathbf{v}}_{i j} \triangleq \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] \\ \triangleq \sum_{k=i}^{j-2}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] + \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\\ \triangleq \Delta \tilde{\mathbf{v}}_{i j-1}+ \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t \end{array} Δv~ij≜∑k=ij−1[ΔR~ik⋅(f~k−bia)⋅Δt]≜∑k=ij−2[ΔR~ik⋅(f~k−bia)⋅Δt]+ΔR~ij−1⋅(f~j−1−bia)⋅Δt≜Δv~ij−1+ΔR~ij−1⋅(f~j−1−bia)⋅Δt -
Δ p ~ i j − 1 → Δ p ~ i j \Delta \tilde{p}_{ij-1} \to \Delta \tilde{p}_{ij} Δp~ij−1→Δp~ij
Δ p ~ i j ≜ ∑ k = i j − 1 [ Δ v ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ] ≜ ∑ k = i j − 2 [ Δ v ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ] + Δ v ~ i j − 1 Δ t + 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) Δ t 2 ≜ Δ p ~ i j − 1 + Δ v ~ i j − 1 Δ t + 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) Δ t 2 \begin{array}{l} \Delta \tilde{\mathbf{p}}_{i j} \triangleq \sum_{k = i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right] \\ \triangleq \sum_{k = i}^{j-2}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right] + \Delta \tilde{\mathbf{v}}_{i j-1} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \\ \triangleq \Delta \tilde{\mathbf{p}}_{i j-1} + \Delta \tilde{\mathbf{v}}_{i j-1} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\mathbf{b}_{i}^{a}\right) \Delta t^{2} \end{array} Δp~ij≜∑k=ij−1[Δv~ikΔt+21ΔR~ik⋅(f~k−bia)Δt2]≜∑k=ij−2[Δv~ikΔt+21ΔR~ik⋅(f~k−bia)Δt2]+Δv~ij−1Δt+21ΔR~ij−1⋅(f~j−1−bia)Δt2≜Δp~ij−1+Δv~ij−1Δt+21ΔR~ij−1⋅(f~j−1−bia)Δt2
预积分测量值更新
当bias发生变化时，利用线性化来进行bias变化时预积分项的一阶近似更新
- bias更新
- b ˉ \bar{b} bˉ：旧的bias
- b ^ \hat{b} b^：新的bias
- δ b \delta b δb：bias更新量
b ^ i g ← b ˉ i g + δ b i g b ^ i a ← b ˉ i a + δ b i a \begin{array}{c} \hat{b} _{i}^{g}\gets \bar{b} _{i}^{g}+\delta b_{i}^{g} \\ \hat{b} _{i}^{a}\gets \bar{b} _{i}^{a}+\delta b_{i}^{a} \end{array} b^ig←bˉig+δbigb^ia←bˉia+δbia
- 一阶近似更新：
Δ R ~ i j ( b ^ i g ) ≈ Δ R ~ i j ( b ‾ i g ) ⋅ Exp ( ∂ Δ R ‾ i j ∂ b ‾ g δ b i g ) Δ v ~ i j ( b ^ i g , b ^ i a ) ≈ Δ v ~ i j ( b ‾ i g , b ‾ i a ) + ∂ Δ v ‾ i j ∂ b ‾ g δ b i g + ∂ Δ v ‾ i j ∂ b ‾ a δ b i a Δ p ~ i j ( b ^ i g , b ^ i a ) ≈ Δ p ~ i j ( b ‾ i g , b ‾ i a ) + ∂ Δ p ‾ i j ∂ b ‾ g δ b i g + ∂ Δ p ‾ i j ∂ b ‾ a δ b i a \begin{array}{l} \Delta \tilde{\mathbf{R}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}\right) \approx \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \\ \Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \approx \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \\ \Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \approx \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \end{array} ΔR~ij(b^ig)≈ΔR~ij(big)⋅Exp(∂bg∂ΔRijδbig)Δv~ij(b^ig,b^ia)≈Δv~ij(big,bia)+∂bg∂Δvijδbig+∂ba∂ΔvijδbiaΔp~ij(b^ig,b^ia)≈Δp~ij(big,bia)+∂bg∂Δpijδbig+∂ba∂Δpijδbia - 符号简化：
Δ R ^ i j ≐ Δ R ~ i j ( b ^ i g ) , Δ R ‾ i j ≐ Δ R ~ i j ( b ‾ i g ) Δ v ^ i j ≐ Δ v ~ i j ( b ^ i g , b ^ i a ) , Δ v ‾ i j ≐ Δ v ~ i j ( b ‾ i g , b ‾ i a ) Δ p ^ i j ≐ Δ p ~ i j ( b ^ i g , b ^ i a ) , Δ p ‾ i j ≐ Δ p ~ i j ( b ‾ i g , b ‾ i a ) \begin{array}{l} \Delta \hat{\mathbf{R}}_{i j} \doteq \Delta \tilde{\mathbf{R}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}\right), \Delta \overline{\mathbf{R}}_{i j} \doteq \Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \\ \Delta \hat{\mathbf{v}}_{i j} \doteq \Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right), \Delta \overline{\mathbf{v}}_{i j} \doteq \Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right) \\ \Delta \hat{\mathbf{p}}_{i j} \doteq \Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right), \Delta \overline{\mathbf{p}}_{i j} \doteq \Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right) \end{array} ΔR^ij≐ΔR~ij(b^ig),ΔRij≐ΔR~ij(big)Δv^ij≐Δv~ij(b^ig,b^ia),Δvij≐Δv~ij(big,bia)Δp^ij≐Δp~ij(b^ig,b^ia),Δpij≐Δp~ij(big,bia) - 更新公式可以简化为：
Δ R ^ i j ≈ Δ R ‾ i j ⋅ Exp ( ∂ Δ R ‾ i j ∂ b ‾ g δ b i g ) Δ v ^ i j ≈ Δ v ‾ i j + ∂ Δ v ‾ i j ∂ b ‾ g δ b i g + ∂ Δ v ‾ i j ∂ b ‾ a δ b i a Δ p ^ i j ≈ Δ p ‾ i j + ∂ Δ p ‾ i j ∂ b ‾ g δ b i g + ∂ Δ p ‾ i j ∂ b ‾ a δ b i a \begin{array}{l} \Delta \hat{\mathbf{R}}_{i j} \approx \Delta \overline{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \\ \Delta \hat{\mathbf{v}}_{i j} \approx \Delta \overline{\mathbf{v}}_{i j}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \\ \Delta \hat{\mathbf{p}}_{i j} \approx \Delta \overline{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a} \end{array} ΔR^ij≈ΔRij⋅Exp(∂bg∂ΔRijδbig)Δv^ij≈Δvij+∂bg∂Δvijδbig+∂ba∂ΔvijδbiaΔp^ij≈Δpij+∂bg∂Δpijδbig+∂ba∂Δpijδbia - ∂ Δ R ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂ΔRij
Δ R ^ i j = Δ R ~ i j ( b ^ i g ) = ∏ k = i j − 1 Exp ( ( ω ~ k − b ^ i g ) Δ t ) = ∏ k = i j − 1 Exp ( ( ω ~ k − ( b ‾ i g + δ b i g ) ) Δ t ) = ∏ k = i j − 1 Exp ( ( ω ~ k − b ‾ i g ) Δ t − δ b i g Δ t ) ≈ ( 1 ) ∏ k = i j − 1 ( Exp ( ( ω ~ k − b ‾ i g ) Δ t ) ⋅ Exp ( − J r k δ b i g Δ t ) ) ⏟ Exp ( ϕ ⃗ + δ ϕ ⃗ ) ≈ Exp ( ϕ ⃗ ) ⋅ Exp ( J r ( ϕ ⃗ ) ⋅ δ ϕ ⃗ ) a n d Exp ( ϕ ⃗ ) ⋅ R = R ⋅ Exp ( R T ϕ ⃗ ) = Δ R ‾ i j ∏ k = i j − 1 Exp ( − Δ R ‾ k + 1 j T J r k δ b i g Δ t ) \begin{aligned} \Delta \hat{\mathbf{R}}_{i j} & =\Delta \tilde{\mathbf{R}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}\right) \\ & =\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\hat{\mathbf{b}}_{i}^{g}\right) \Delta t\right) \\ & =\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\left(\overline{\mathbf{b}}_{i}^{g}+\delta \mathbf{b}_{i}^{g}\right)\right) \Delta t\right) \\ & =\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}\right) \Delta t-\delta \mathbf{b}_{i}^{g} \Delta t\right) \\ & \stackrel{(1)}{\approx}\underbrace{ \prod_{k=i}^{j-1}\left(\operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\overline{\mathbf{b}}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{k} \delta \mathbf{b}_{i}^{g} \Delta t\right)\right)}_{\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_{r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) and \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R} = \mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right)} \\ & =\Delta \overline{\mathbf{R}}_{i j} \prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \delta \mathbf{b}_{i}^{g} \Delta t\right) \end{aligned} ΔR^ij=ΔR~ij(b^ig)=k=i∏j−1Exp((ω~k−b^ig)Δt)=k=i∏j−1Exp((ω~k−(big+δbig))Δt)=k=i∏j−1Exp((ω~k−big)Δt−δbigΔt)≈(1)Exp(ϕ+δϕ)≈Exp(ϕ)⋅Exp(Jr(ϕ)⋅δϕ)andExp(ϕ)⋅R=R⋅Exp(RTϕ) k=i∏j−1(Exp((ω~k−big)Δt)⋅Exp(−JrkδbigΔt))=ΔRijk=i∏j−1Exp(−ΔRk+1jTJrkδbigΔt)
可以得到，
∂ Δ R ‾ i j ∂ b ‾ g = ∑ k = i j − 1 ( − Δ R ‾ k + 1 j T J r k Δ t ) \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_{k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) ∂bg∂ΔRij=k=i∑j−1(−ΔRk+1jTJrkΔt)
其中，
J r k = J r ( ( ω ~ k − b i g ) Δ t ) \mathbf{J}_{r}^{k}=\mathbf{J}_{r}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) Jrk=Jr((ω~k−big)Δt)
- ∂ Δ v ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂Δvij和 ∂ Δ v ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} ∂ba∂Δvij
将 Δ R ^ i j = Δ R ‾ i j Exp ( ∑ k = i j − 1 ( − Δ R ‾ k + 1 j T J r k δ b i g Δ t ) ) \Delta \hat{\mathbf{R}}_{i j}=\Delta \overline{\mathbf{R}}_{i j} \operatorname{Exp}\left(\sum_{k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \delta \mathbf{b}_{i}^{g} \Delta t\right)\right) ΔR^ij=ΔRijExp(∑k=ij−1(−ΔRk+1jTJrkδbigΔt))代入
Δ v ^ i j = Δ v ~ i j ( b ^ i g , b ^ i a ) = ∑ k = i j − 1 [ Δ R ~ i k ( b ^ i g ) ⋅ ( f ~ k − b ^ i a ) Δ t ] ≈ ∑ k = i j − 1 [ Δ R ‾ i k ⋅ Exp ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ⋅ ( f ~ k − b ‾ i a − δ b i a ) Δ t ] ≈ ( 1 ) ∑ k = i j − 1 [ Δ R ‾ i k ⋅ ( I + ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ∧ ) ⋅ ( f ~ k − b ‾ i a − δ b i a ) Δ t ] = ∑ k = i j − 1 [ Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) Δ t − Δ R ‾ i k δ b i a Δ t + Δ R ‾ i k ⋅ ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ∧ ( f ~ k − b ‾ i a ) Δ t − Δ R ‾ i k ⋅ ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ∧ δ b i a Δ t ] ≈ ( 2 ) Δ v ‾ i j + ∑ k = i j − 1 { − [ Δ R ‾ i k Δ t ] δ b i a − [ Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ] δ b i g } \begin{array}{l} \Delta \hat{\mathbf{v}}_{i j}=\Delta \tilde{\mathbf{v}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \\ =\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right) \Delta t\right] \\ \approx \sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t\right] \\ \stackrel{(1)}{\approx} \sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right) \Delta t\right] \\ =\sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t-\Delta \overline{\mathbf{R}}_{i k} \delta \mathbf{b}_{i}^{a} \Delta t+\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t-\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge} \delta \mathbf{b}_{i}^{a} \Delta t\right] \\ \stackrel{(2)}{\approx} \Delta \overline{\mathbf{v}}_{i j}+\sum_{k=i}^{j-1}\left\{-\left[\Delta \overline{\mathbf{R}}_{i k} \Delta t\right] \delta \mathbf{b}_{i}^{a}-\left[\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right] \delta \mathbf{b}_{i}^{g}\right\} \\ \end{array} Δv^ij=Δv~ij(b^ig,b^ia)=∑k=ij−1[ΔR~ik(b^ig)⋅(f~k−b^ia)Δt]≈∑k=ij−1[ΔRik⋅Exp(∂bg∂ΔRikδbig)⋅(f~k−bia−δbia)Δt]≈(1)∑k=ij−1[ΔRik⋅(I+(∂bg∂ΔRikδbig)∧)⋅(f~k−bia−δbia)Δt]=∑k=ij−1[ΔRik⋅(f~k−bia)Δt−ΔRikδbiaΔt+ΔRik⋅(∂bg∂ΔRikδbig)∧(f~k−bia)Δt−ΔRik⋅(∂bg∂ΔRikδbig)∧δbiaΔt]≈(2)Δvij+∑k=ij−1{−[ΔRikΔt]δbia−[ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt]δbig}
可以得到：
∂ Δ v ‾ i j ∂ b ‾ g = − ∑ k = i j − 1 ( Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ) ∂ Δ v ‾ i j ∂ b ‾ a = − ∑ k = i j − 1 ( Δ R ‾ i k Δ t ) \begin{aligned} \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} & =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \\ \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} & =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right) \end{aligned} ∂bg∂Δvij∂ba∂Δvij=−k=i∑j−1(ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt)=−k=i∑j−1(ΔRikΔt) - ∂ Δ p ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂Δpij和 ∂ Δ p ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} ∂ba∂Δpij
Δ p ^ i j = Δ p ~ i j ( b ^ i g , b ^ i a ) = ∑ k = i j − 1 [ Δ v ~ i k ( b ^ i g , b ^ i a ) Δ t + 1 2 Δ R ~ i k ( b ^ i g ) ⋅ ( f ~ k − b ^ i a ) Δ t 2 ] = ∑ k = i j − 1 [ Δ v ~ i k ( b ^ i g , b ^ i a ) Δ t ] ⏟ 1 + 1 2 ∑ k = i j − 1 [ Δ R ~ i k ( b ^ i g ) ⋅ ( f ~ k − b ^ i a ) Δ t 2 ] ⏟ 2 \begin{aligned} \Delta \hat{\mathbf{p}}_{i j} & =\Delta \tilde{\mathbf{p}}_{i j}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \\ & =\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right) \Delta t^{2}\right] \\ & =\underbrace{\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}, \hat{\mathbf{b}}_{i}^{a}\right) \Delta t\right]}_{1}+\underbrace{\frac{1}{2} \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right) \Delta t^{2}\right]}_{2} \end{aligned} Δp^ij=Δp~ij(b^ig,b^ia)=k=i∑j−1[Δv~ik(b^ig,b^ia)Δt+21ΔR~ik(b^ig)⋅(f~k−b^ia)Δt2]=1 k=i∑j−1[Δv~ik(b^ig,b^ia)Δt]+2 21k=i∑j−1[ΔR~ik(b^ig)⋅(f~k−b^ia)Δt2]
对于1和2分别推导：
( 1 ) = ∑ k = i j − 1 [ ( Δ v ‾ i k + ∂ Δ v ‾ i k ∂ b ‾ g δ b i g + ∂ Δ v ‾ i k ∂ b ‾ a δ b i a ) Δ t ] = ∑ k = i j − 1 [ Δ v ‾ i k Δ t + ( ∂ Δ v ‾ i k ∂ b ‾ g Δ t ) δ b i g + ( ∂ Δ v ‾ i k ∂ b ‾ a Δ t ) δ b i a ] \begin{aligned} (1) & =\sum_{k=i}^{j-1}\left[\left(\Delta \overline{\mathbf{v}}_{i k}+\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \delta \mathbf{b}_{i}^{a}\right) \Delta t\right] \\ & =\sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{v}}_{i k} \Delta t+\left(\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \delta \mathbf{b}_{i}^{g}+\left(\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t\right) \delta \mathbf{b}_{i}^{a}\right] \end{aligned} (1)=k=i∑j−1[(Δvik+∂bg∂Δvikδbig+∂ba∂Δvikδbia)Δt]=k=i∑j−1[ΔvikΔt+(∂bg∂ΔvikΔt)δbig+(∂ba∂ΔvikΔt)δbia]
( 2 ) ≈ Δ t 2 2 ∑ k = i j − 1 [ Δ R ~ i k ( b ^ i g ) ⋅ ( f ~ k − b ^ i a ) ] = Δ t 2 2 ∑ k = i j − 1 [ Δ R ‾ i k ⋅ Exp ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ⋅ ( f ~ k − b ‾ i a − δ b i a ) ] ≈ Δ t 2 2 ∑ k = i j − 1 [ Δ R ‾ i k ⋅ ( I + ( ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ) ∧ ) ⋅ ( f ~ k − b ‾ i a − δ b i a ) ] ≈ Δ t 2 2 ∑ k = i j − 1 [ Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) − Δ R ‾ i k δ b i a − Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g δ b i g ] \begin{aligned} (2) & \approx \frac{\Delta t^{2}}{2} \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k}\left(\hat{\mathbf{b}}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\hat{\mathbf{b}}_{i}^{a}\right)\right] \\ & =\frac{\Delta t^{2}}{2} \sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right)\right] \\ & \approx \frac{\Delta t^{2}}{2} \sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}+\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right)^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}-\delta \mathbf{b}_{i}^{a}\right)\right] \\ & \approx \frac{\Delta t^{2}}{2} \sum_{k=i}^{j-1}\left[\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)-\Delta \overline{\mathbf{R}}_{i k} \delta \mathbf{b}_{i}^{a}-\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \delta \mathbf{b}_{i}^{g}\right] \end{aligned} (2)≈2Δt2k=i∑j−1[ΔR~ik(b^ig)⋅(f~k−b^ia)]=2Δt2k=i∑j−1[ΔRik⋅Exp(∂bg∂ΔRikδbig)⋅(f~k−bia−δbia)]≈2Δt2k=i∑j−1[ΔRik⋅(I+(∂bg∂ΔRikδbig)∧)⋅(f~k−bia−δbia)]≈2Δt2k=i∑j−1[ΔRik⋅(f~k−bia)−ΔRikδbia−ΔRik⋅(f~k−bia)∧∂bg∂ΔRikδbig]
将1与2组合：
Δ p ^ i j = ∑ k = i j − 1 { [ Δ v ‾ i k Δ t + 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) Δ t 2 ] + [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] δ b i g + [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] δ b i a } = Δ p ‾ i j + { ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] } δ b i g + { ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] } δ b i a \begin{array}{l} \Delta \hat{\mathbf{p}}_{i j} =\sum_{k=i}^{j-1}\left\{\left[\Delta \overline{\mathbf{v}}_{i k} \Delta t+\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right) \Delta t^{2}\right]+\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \delta \mathbf{b}_{i}^{g}+\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] \delta \mathbf{b}_{i}^{a}\right\} \\ =\Delta \overline{\mathbf{p}}_{i j}+\left\{\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right]\right\} \delta \mathbf{b}_{i}^{g}+\left\{\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right]\right\} \delta \mathbf{b}_{i}^{a} \end{array} Δp^ij=∑k=ij−1{[ΔvikΔt+21ΔRik⋅(f~k−bia)Δt2]+[∂bg∂ΔvikΔt−21ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt2]δbig+[∂ba∂ΔvikΔt−21ΔRikΔt2]δbia}=Δpij+{∑k=ij−1[∂bg∂ΔvikΔt−21ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt2]}δbig+{∑k=ij−1[∂ba∂ΔvikΔt−21ΔRikΔt2]}δbia
得到：
∂ Δ p ‾ i j ∂ b ‾ g = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] ∂ Δ p ‾ i j ∂ b ‾ a = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] \begin{aligned} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} & =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \\ \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} & =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] \end{aligned} ∂bg∂Δpij∂ba∂Δpij=k=i∑j−1[∂bg∂ΔvikΔt−21ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt2]=k=i∑j−1[∂ba∂ΔvikΔt−21ΔRikΔt2]
Jacobian更新
- ∂ Δ v ‾ i j − 1 ∂ b ‾ g → ∂ Δ v ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂Δvij−1→∂bg∂Δvij
∂ Δ v ‾ i j ∂ b ‾ g = − ∑ k = i j − 1 ( Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ) = − ∑ k = i j − 2 ( Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t ) − Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t = ∂ Δ v ‾ i j − 1 ∂ b ‾ g − Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) \\ =-\sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t\right) - \Delta \overline{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \Delta t \\ = \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}-{\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Yellow} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t \end{array} ∂bg∂Δvij=−∑k=ij−1(ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt)=−∑k=ij−2(ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt)−ΔRij−1⋅(f~j−1−bia)∧∂bg∂ΔRij−1Δt=∂bg∂Δvij−1−ΔRij−1⋅(f~j−1−bia)∧∂bg∂ΔRij−1Δt - ∂ Δ v ‾ i j − 1 ∂ b ‾ a → ∂ Δ v ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} \to \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} ∂ba∂Δvij−1→∂ba∂Δvij
∂ Δ v ‾ i j ∂ b ‾ a = − ∑ k = i j − 1 ( Δ R ‾ i k Δ t ) = − ∑ k = i j − 2 ( Δ R ‾ i k Δ t ) − Δ R ‾ i j − 1 Δ t = ∂ Δ v ‾ i j − 1 ∂ b ‾ a − Δ R ‾ i j − 1 Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} =-\sum_{k=i}^{j-1}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right) \\ =-\sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{i k} \Delta t\right)-\Delta \overline{\mathbf{R}}_{i j-1} \Delta t \\ =\frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} -{\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t \end{array} ∂ba∂Δvij=−∑k=ij−1(ΔRikΔt)=−∑k=ij−2(ΔRikΔt)−ΔRij−1Δt=∂ba∂Δvij−1−ΔRij−1Δt
- ∂ Δ p ‾ i j − 1 ∂ b ‾ g → ∂ Δ p ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂Δpij−1→∂bg∂Δpij
∂ Δ p ‾ i j ∂ b ‾ g = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] = ∑ k = i j − 2 [ ∂ Δ v ‾ i k ∂ b ‾ g Δ t − 1 2 Δ R ‾ i k ⋅ ( f ~ k − b ‾ i a ) ∧ ∂ Δ R ‾ i k ∂ b ‾ g Δ t 2 ] + ∂ Δ v ‾ i j − 1 ∂ b ‾ g Δ t − 1 2 Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t 2 = ∂ Δ p ‾ i j − 1 ∂ b ‾ g + ∂ Δ v ‾ i j − 1 ∂ b ‾ g Δ t − 1 2 Δ R ‾ i j − 1 ⋅ ( f ~ j − 1 − b ‾ i a ) ∧ ∂ Δ R ‾ i j − 1 ∂ b ‾ g Δ t 2 \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] \\ =\sum_{k=i}^{j-2}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} \frac{\partial \Delta \overline{\mathbf{R}}_{i k}}{\partial \overline{\mathbf{b}}^{g}} \Delta t^{2}\right] + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Yellow} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} } \Delta t^{2} \\ =\frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1}} \cdot\left(\tilde{\mathbf{f}}_{j-1}-\overline{\mathbf{b}}_{i}^{a}\right)^{\wedge} {\color{Yellow} \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} } \Delta t^{2} \end{array} ∂bg∂Δpij=∑k=ij−1[∂bg∂ΔvikΔt−21ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt2]=∑k=ij−2[∂bg∂ΔvikΔt−21ΔRik⋅(f~k−bia)∧∂bg∂ΔRikΔt2]+∂bg∂Δvij−1Δt−21ΔRij−1⋅(f~j−1−bia)∧∂bg∂ΔRij−1Δt2=∂bg∂Δpij−1+∂bg∂Δvij−1Δt−21ΔRij−1⋅(f~j−1−bia)∧∂bg∂ΔRij−1Δt2
- ∂ Δ p ‾ i j − 1 ∂ b ‾ a → ∂ Δ p ‾ i j ∂ b ‾ a \frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} \to \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} ∂ba∂Δpij−1→∂ba∂Δpij
∂ Δ p ‾ i j ∂ b ‾ a = ∑ k = i j − 1 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] = ∑ k = i j − 2 [ ∂ Δ v ‾ i k ∂ b ‾ a Δ t − 1 2 Δ R ‾ i k Δ t 2 ] + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 = ∂ Δ p ‾ i j − 1 ∂ b ‾ a + ∂ Δ v ‾ i j − 1 ∂ b ‾ a Δ t − 1 2 Δ R ‾ i j − 1 Δ t 2 \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \overline{\mathbf{b}}^{a}} =\sum_{k=i}^{j-1}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] \\ =\sum_{k=i}^{j-2}\left[\frac{\partial \Delta \overline{\mathbf{v}}_{i k}}{\partial \overline{\mathbf{b}}^{a}} \Delta t-\frac{1}{2} \Delta \overline{\mathbf{R}}_{i k} \Delta t^{2}\right] + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \\ =\frac{\partial \Delta \overline{\mathbf{p}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}} + {\color{Red} \frac{\partial \Delta \overline{\mathbf{v}}_{i j-1}}{\partial \overline{\mathbf{b}}^{a}}} \Delta t-\frac{1}{2} {\color{Green} \Delta \overline{\mathbf{R}}_{i j-1} } \Delta t^{2} \end{array} ∂ba∂Δpij=∑k=ij−1[∂ba∂ΔvikΔt−21ΔRikΔt2]=∑k=ij−2[∂ba∂ΔvikΔt−21ΔRikΔt2]+∂ba∂Δvij−1Δt−21ΔRij−1Δt2=∂ba∂Δpij−1+∂ba∂Δvij−1Δt−21ΔRij−1Δt2
- ∂ Δ R ‾ i j − 1 ∂ b ‾ g → ∂ Δ R ‾ i j ∂ b ‾ g \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} \to \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}} ∂bg∂ΔRij−1→∂bg∂ΔRij
∂ Δ R ‾ i j ∂ b ‾ g = ∑ k = i j − 1 ( − Δ R ‾ k + 1 j T J r k Δ t ) = ∑ k = i j − 2 ( − Δ R ‾ k + 1 j T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = ∑ k = i j − 2 ( − ( Δ R ‾ k + 1 j − 1 Δ R ‾ j − 1 j ) T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = − Δ R ‾ j − 1 j T ⋅ ∑ k = i j − 2 ( Δ R ‾ k + 1 j − 1 T J r k Δ t ) − Δ R ‾ j j T J r k Δ t = − Δ R ‾ j − 1 j T ⋅ ∂ Δ R ‾ i j − 1 ∂ b ‾ g − Δ R ‾ j j T ⏟ I J r k Δ t \begin{array}{l} \frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \overline{\mathbf{b}}^{g}}=\sum_{k=i}^{j-1}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) \\ =\sum_{k=i}^{j-2}\left(-\Delta \overline{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ =\sum_{k=i}^{j-2}\left(-\left(\Delta \overline{\mathbf{R}}_{k+1 j-1}\Delta \overline{\mathbf{R}}_{j-1 j} \right)^{T}\mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ =-\Delta \overline{\mathbf{R}}_{ j-1 j}^{T}\cdot \sum_{k=i}^{j-2}\left(\Delta \overline{\mathbf{R}}_{k+1 j-1}^{T} \mathbf{J}_{r}^{k} \Delta t\right) -\Delta \overline{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{k} \Delta t \\ = -\Delta \overline{\mathbf{R}}_{ j-1 j}^{T}\cdot \frac{\partial \Delta \overline{\mathbf{R}}_{i j-1}}{\partial \overline{\mathbf{b}}^{g}} -\underbrace{\Delta \overline{\mathbf{R}}_{j j}^{T}}_{I} \mathbf{J}_{r}^{k} \Delta t \end{array} ∂bg∂ΔRij=∑k=ij−1(−ΔRk+1jTJrkΔt)=∑k=ij−2(−ΔRk+1jTJrkΔt)−ΔRjjTJrkΔt=∑k=ij−2(−(ΔRk+1j−1ΔRj−1j)TJrkΔt)−ΔRjjTJrkΔt=−ΔRj−1jT⋅∑k=ij−2(ΔRk+1j−1TJrkΔt)−ΔRjjTJrkΔt=−ΔRj−1jT⋅∂bg∂ΔRij−1−I ΔRjjTJrkΔt
这篇关于ORB_SLAM3_IMU预积分理论推导(更新)的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
