本博客借鑑了崔華坤的《VINS論文推導及代碼解析》和 VINS-Mono理論學習——IMU預積分 Pre-integration (Jacobian 協方差) 的內容，因爲確實寫得太好了，然後有些地方加入自己一些理解。
VINS-MONO論文中的IV-B. IMU Pre-integration介紹了IMU預積分模型，Foster的倆篇論文對IMU預積分理論進行詳細分析。
爲什麼需要對IMU進行預積分？ k k k k + 1 k+1 k + 1
然而在非線性優化的VIO中，各個節點的狀態量都是估計值，當算法對這些狀態量優化時，每次調整都需要在它們之間重新積分，導致絕對位姿被優化時對狀態量進行重複積分。IMU預積分的提出使得優化算法可對IMU的相對測量進行處理，使它與絕對位姿解耦，或者只要線性運算就可以進行矯正 。
IMU測量值包括加速度計得到的 (測量值) 線加速度a t ^ \hat{a_{t}} a t ^ w ^ t \hat{w}_{t} w ^ t a ^ t = a t + b a t + R w t g w + n a \hat{a}_{t}=a_{t}+b_{at}+R_{w}^{t}g^{w}+n_{a} a ^ t = a t + b a t + R w t g w + n a w ^ t = w t + b w t + n w \hat{w}_{t}=w_{t}+b_{wt}+n_{w} w ^ t = w t + b w t + n w t t t a t a_{t} a t w t w_{t} w t b a t b_{at} b a t b w t b_{wt} b w t n a n_a n a n n w n_{n_{w}} n n w a t ^ \hat{a_{t}} a t ^ n a ∼ ( 0 , σ a 2 ) , n a ∼ ( 0 , σ w 2 ) n_{a}\sim(0,\sigma_{a}^{2}), \ n_{a}\sim(0,\sigma_{w}^{2}) n a ∼ ( 0 , σ a 2 ) , n a ∼ ( 0 , σ w 2 ) b ˙ a t = n b a , b ˙ w t = n b w \dot{b}_{at}=n_{ba}, \ \dot{b}_{wt}=n_{bw} b ˙ a t = n b a , b ˙ w t = n b w n b a ∼ N ( 0 , σ b a 2 ) , n b w ∼ N ( 0 , σ b w 2 ) n_{ba}\sim N(0,\sigma_{ba}^{2}), \ n_{bw} \sim N(0, \sigma_{bw}^{2}) n b a ∼ N ( 0 , σ b a 2 ) , n b w ∼ N ( 0 , σ b w 2 )
對於圖像幀k k k k + 1 k+1 k + 1 b k b_{k} b k b k + 1 b_{k+1} b k + 1 位置、速度和姿態狀態值PVQ(Pose、Velocity、Quaternion)可以根據[ t k , t k + 1 ] [t_{k}, t_{k+1}] [ t k , t k + 1 ] ：【論文式(3)(4)】p b k + 1 w = p b k w + v b k w Δ t k + ∫ ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 p^{w}_{b_{k+1}}=p^{w}_{bk}+v_{b_{k}}^{w}\Delta t_{k}+\int \int_{t\in[t_{k}, t_{k+1}]}(R_{t}^{w}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt^{2} p b k + 1 w = p b k w + v b k w Δ t k + ∫ ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t v_{b_{k+1}}^{w}=v_{bk}^{w}+\int_{t\in[t_{k},t_{k+1}]}(R^{w}_{t}(\hat{a}_{t}-b_{at}-n_{a})-g^{w})dt v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ ( w ^ t − b w t − n w ) 0 ] d t = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) q t b k d t (1) q_{b_{k+1}}^{w} = q_{b_{k}}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix}
(\hat{w}_{t}-b_{wt}-n_{w})\\
0
\end{bmatrix}dt \\ = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt \tag{1} q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 2 1 q t b k ⊗ [ ( w ^ t − b w t − n w ) 0 ] d t = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 2 1 Ω ( w ^ t − b w t − n w ) q t b k d t ( 1 ) Ω ( w ) = [ − [ w ] × w − w T 0 ] , [ w ] × = [ 0 − w z w y w z 0 − w x − w y w x 0 ] \Omega(w)=\begin{bmatrix}
-[w]_{\times} & w\\
-w^{T}& 0
\end{bmatrix}, \ [w]_{\times} = \begin{bmatrix}
0 & -w_{z} & w_{y}\\
w_{z} & 0 & -w_{x}\\
-w_{y} & w_{x} & 0
\end{bmatrix} Ω ( w ) = [ − [ w ] × − w T w 0 ] , [ w ] × = ⎣ ⎡ 0 w z − w y − w z 0 w x w y − w x 0 ⎦ ⎤ Δ t k \Delta t_{k} Δ t k [ t k , t k + 1 ] [t_{k}, t_{k+1}] [ t k , t k + 1 ] R t w R_{t}^{w} R t w q t b k q_{t}^{bk} q t b k t t t k k k 四元素實部在後，虛部在前, 爲了與論文保持一致 。這裏的Ω ( w ) \Omega(w) Ω ( w )
關於公式( 1 ) (1) ( 1 ) 左乘和右乘符號 如下：q a ⊗ q b = R ( q b ) q a = [ s b z b − y b x b − z b s b x b y b y b − x b s b z b − x b − y b − z b s b ] [ x a y a z a s a ] = L ( q a ) q b = [ s a − z a y a x a z a s a − x a y a − y a x a s a z a − x a − y a − z a s a ] [ x b y b z b s b ] q_{a} \otimes q_{b} = R(q_{b})q_{a} = \begin{bmatrix}
s_{b} & z_{b} & -y_{b} & x_{b}\\
-z_{b} & s_{b} & x_{b} & y_{b}\\
y_{b}& -x_{b} & s_{b} & z_{b}\\
-x_{b}& -y_{b} & -z_{b} & s_{b}
\end{bmatrix}\begin{bmatrix}
x_{a}\\
y_{a}\\
z_{a}\\
s_{a}
\end{bmatrix} \\ = L(q_{a})q_{b} = \begin{bmatrix}
s_{a} & -z_{a} & y_{a} & x_{a}\\
z_{a} & s_{a} & -x_{a} & y_{a}\\
-y_{a}& x_{a} & s_{a} & z_{a}\\
-x_{a}& -y_{a} & -z_{a} & s_{a}
\end{bmatrix}\begin{bmatrix}
x_{b}\\
y_{b}\\
z_{b}\\
s_{b}
\end{bmatrix} q a ⊗ q b = R ( q b ) q a = ⎣ ⎢ ⎢ ⎡ s b − z b y b − x b z b s b − x b − y b − y b x b s b − z b x b y b z b s b ⎦ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎡ x a y a z a s a ⎦ ⎥ ⎥ ⎤ = L ( q a ) q b = ⎣ ⎢ ⎢ ⎡ s a z a − y a − x a − z a s a x a − y a y a − x a s a − z a x a y a z a s a ⎦ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎡ x b y b z b s b ⎦ ⎥ ⎥ ⎤ q = [ x y z s ] = [ w s ] q=[x \ y \ z \ s] = [w \ s] q = [ x y z s ] = [ w s ] R ( q ) = Ω ( w ) + s I 4 × 4 = [ − [ w ] × w − w T 0 ] + s I 4 × 4 R(q) = \Omega(w)+sI_{4\times4} = \begin{bmatrix}
-[w]_{\times} & w\\
-w^{T}& 0
\end{bmatrix} + sI_{4\times4} R ( q ) = Ω ( w ) + s I 4 × 4 = [ − [ w ] × − w T w 0 ] + s I 4 × 4 L ( q ) = Ψ ( w ) + s I 4 × 4 = [ [ w ] × w − w T 0 ] + s I 4 × 4 L(q) = \Psi(w)+sI_{4\times4} = \begin{bmatrix}
[w]_{\times} & w\\
-w^{T}& 0
\end{bmatrix} + sI_{4\times4} L ( q ) = Ψ ( w ) + s I 4 × 4 = [ [ w ] × − w T w 0 ] + s I 4 × 4 四元素的求導 ，我們定義q t q_{t} q t t t t 這裏的四元素實部在後，虛部在前 )，w w w q t q_{t} q t q t q_{t} q t q ˙ t = 1 2 [ − [ w ] × w − w T 0 ] q t = 1 2 Ω ( w ) q t = 1 2 R ( [ w 0 ] ) q t = 1 2 q t ⊗ [ w 0 ] \dot{q}_{t} = \frac{1}{2} \begin{bmatrix}
-[w]_{\times} & w\\
-w^{T}& 0
\end{bmatrix}q_{t} = \frac{1}{2}\Omega(w)q_{t}=\frac{1}{2}R(\begin{bmatrix}
w\\
0
\end{bmatrix})q_{t} = \frac{1}{2}q_{t} \otimes \begin{bmatrix}
w\\
0
\end{bmatrix} q ˙ t = 2 1 [ − [ w ] × − w T w 0 ] q t = 2 1 Ω ( w ) q t = 2 1 R ( [ w 0 ] ) q t = 2 1 q t ⊗ [ w 0 ]
當看到q ˙ = 1 2 R ( [ 0 w ] ) q t = 1 2 q t ⊗ [ 0 w ] \dot{q} = \frac{1}{2}\mathcal{R}(\begin{bmatrix} 0 \\ w \end{bmatrix}) q_{t}= \frac{1}{2} q_{t} \otimes \begin{bmatrix} 0 \\ w \end{bmatrix} q ˙ = 2 1 R ( [ 0 w ] ) q t = 2 1 q t ⊗ [ 0 w ]
因此對於IMU連續形式下的旋轉狀態(用四元素表示)推導 ，我們有：q b k + 1 w = q b k w ⊗ q b k + 1 b k = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] q t ˙ b k d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ w t b k 0 ] d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ w t ^ − b w t − n w 0 ] d t = = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) q t b k d t q_{b_{k+1}^{w}}=q_{b_{k}}^{w} \otimes q_{b_{k+1}}^{b_{k}} = q_{b_{k}}^{w} \otimes \int _{t\in [t_{k}, t_{k+1}]} \dot{q_{t}}^{b_{k}} dt = q_{bk}^{w} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} q_{t}^{b_{k}}\otimes \begin{bmatrix}
w_{t}^{b_{k}}\\
0\end{bmatrix}dt \\ =q_{b_{k}}^{w} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2}q_{t}^{b_{k}} \otimes \begin{bmatrix}
\hat{w_{t}}-b_{wt}-n_{w}\\
0\end{bmatrix} dt \\ = = q_{bk} \otimes \int_{t\in[t_{k}, t_{k+1}]} \frac{1}{2} \Omega (\hat{w}_{t}-b_{wt}-n_{w})q_{t}^{bk}dt q b k + 1 w = q b k w ⊗ q b k + 1 b k = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] q t ˙ b k d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 2 1 q t b k ⊗ [ w t b k 0 ] d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 2 1 q t b k ⊗ [ w t ^ − b w t − n w 0 ] d t = = q b k ⊗ ∫ t ∈ [ t k , t k + 1 ] 2 1 Ω ( w ^ t − b w t − n w ) q t b k d t
使用歐拉法，即k + 1 k+1 k + 1 k k k a ^ b k \hat{a}_{b_{k}} a ^ b k w ^ b k \hat{w}_{b_{k}} w ^ b k p b k + 1 w = p b k w + v b k w Δ t k + 1 2 a ^ b k δ t 2 p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\hat{a}_{b_{k}}\delta t^{2} p b k + 1 w = p b k w + v b k w Δ t k + 2 1 a ^ b k δ t 2 v b k + 1 w = v b k w + a ^ b k δ t v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \hat{a}_{b_{k}} \delta t v b k + 1 w = v b k w + a ^ b k δ t q b k + 1 w = q b k w ⊗ [ 1 1 2 w ^ b k δ t ] q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix}
1\\
\frac{1}{2}\hat{w}_{b_{k}}\delta t
\end{bmatrix} q b k + 1 w = q b k w ⊗ [ 1 2 1 w ^ b k δ t ] a ^ b k = q b k w ( a b k − b a k ) − g w \hat{a}_{b_{k}} = q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} a ^ b k = q b k w ( a b k − b a k ) − g w w ^ b k = w b k − b w k \hat{w}_{b_{k}} = w_{b_{k}}-b_{wk} w ^ b k = w b k − b w k
使用中值法，即k + 1 k+1 k + 1 k k k k + 1 k+1 k + 1 a a a w w w p b k + 1 w = p b k w + v b k w Δ t k + 1 2 a ^ ˉ t δ t 2 p^{w}_{b_{k+1}} = p_{b_{k}}^{w} + v_{b_{k}}^{w} \Delta t_{k} + \frac{1}{2}\bar{\hat{a}}_{t}\delta t^{2} p b k + 1 w = p b k w + v b k w Δ t k + 2 1 a ^ ˉ t δ t 2 v b k + 1 w = v b k w + a ^ ˉ t δ t v_{b_{k+1}}^{w} = v_{b_{k}}^{w} + \bar{\hat{a}}_{t} \delta t v b k + 1 w = v b k w + a ^ ˉ t δ t q b k + 1 w = q b k w ⊗ [ 1 1 2 w ^ ˉ t δ t ] q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \begin{bmatrix}
1\\
\frac{1}{2}\bar{\hat{w}}_{t}\delta t
\end{bmatrix} q b k + 1 w = q b k w ⊗ [ 1 2 1 w ^ ˉ t δ t ] a ^ ˉ t = 1 2 [ q b k w ( a b k − b a k ) − g w + q b k + 1 w ( a b k + 1 − b a k + 1 ) − g w ] \bar{\hat{a}}_{t} =
\frac{1}{2}[q_{b_{k}}^{w}(a_{b_{k}}-b_{ak})-g^{w} + q_{b_{k+1}}^{w}(a_{b_{k+1}}-b_{ak+1})-g^{w}] a ^ ˉ t = 2 1 [ q b k w ( a b k − b a k ) − g w + q b k + 1 w ( a b k + 1 − b a k + 1 ) − g w ] w ^ ˉ t = 1 2 ( w b k − b w k + w b k + 1 − b w k + 1 ) = 1 2 ( w b k + w b k ) − b w k \bar{\hat{w}}_{t} = \frac{1}{2}(w_{b_{k}}-b_{wk}+w_{b_{k+1}}-b_{wk+1}) \\ = \frac{1}{2}(w_{b_{k}}+w_{b_{k}})-b_{w{k}} w ^ ˉ t = 2 1 ( w b k − b w k + w b k + 1 − b w k + 1 ) = 2 1 ( w b k + w b k ) − b w k b a k = b a k + 1 , b w k = b w k + 1 b_{ak}=b_{ak+1}, \ b_{wk} = b_{wk+1} b a k = b a k + 1 , b w k = b w k + 1
通過公式( 1 ) (1) ( 1 ) k k k v v v R R R k k k v v v R R R k k k k + 1 k+1 k + 1
IMU預積分的思想就是將參考座標系從世界座標系w w w k k k b k b_{k} b k R w b k R^{b_{k}}_{w} R w b k R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R^{b_{k}}_{w}p^{w}_{b_{k+1}}=R^{b_{k}}_{w}(p_{b_{k}}^{w}+v^{w}_{b_{k}}\Delta t_{k}-\frac{1}{2}g^{w}\Delta t_{k}^{2}) + \alpha^{b_{k}}_{b_{k+1}} R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 2 1 g w Δ t k 2 ) + α b k + 1 b k R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k R^{b_{k}}_{w}v_{b_{k+1}}^{w} = R_{w}^{b_{k}}(v_{b_{k}}^{w}-g^{w}\Delta t_{k})+\beta^{b_{k}}_{b_{k+1}} R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k q w b k ⊗ q b k + 1 w = γ b k + 1 b k (2) q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w} = \gamma _{b_{k+1}} ^{b_{k}} \tag{2} q w b k ⊗ q b k + 1 w = γ b k + 1 b k ( 2 ) α b k + 1 b k = ∫ ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 \alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2} α b k + 1 b k = ∫ ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t \beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t γ b k + 1 = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) γ t b k d t \gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt γ b k + 1 = ∫ t ∈ [ t k , t k + 1 ] 2 1 Ω ( w ^ t − b w t − n w ) γ t b k d t
此時的積分結果α b k + 1 b k \alpha^{b_{k}}_{b_{k+1}} α b k + 1 b k β b k + 1 b k \beta^{b_{k}}_{b_{k+1}} β b k + 1 b k γ b k + 1 b k \gamma^{b_{k}}_{b_{k+1}} γ b k + 1 b k b k + 1 b_{k+1} b k + 1 b k b_{k} b k b k b_{k} b k
注意，這是在假設IMU偏置b a b_{a} b a b w b_{w} b w b a b_{a} b a b w b_{w} b w
當偏置變換很小時，可以將α b k + 1 b k \alpha_{b_{k+1}}^{b_{k}} α b k + 1 b k β b k + 1 b k \beta_{b_{k+1}}^{b_{k}} β b k + 1 b k γ b k + 1 b k \gamma_{b_{k+1}}^{b_{k}} γ b k + 1 b k α b k + 1 b k ≈ α ^ b k + 1 b k + J b a α δ b a + J b w α δ b w \alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \alpha} \delta b_{a} + J_{b_{w}}^{ \alpha} \delta b_{w} α b k + 1 b k ≈ α ^ b k + 1 b k + J b a α δ b a + J b w α δ b w β b k + 1 b k ≈ β ^ b k + 1 b k + J b a β δ b a + J b w β δ b w \beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}} + J_{b_{a}}^{ \beta} \delta b_{a} + J_{b_{w}}^{ \beta} \delta b_{w} β b k + 1 b k ≈ β ^ b k + 1 b k + J b a β δ b a + J b w β δ b w γ b k + 1 b k ≈ γ ^ b k + 1 b k [ 1 1 2 J b w γ δ b w ] \gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_{w}}^{\gamma}\delta b_{w} \end{bmatrix} γ b k + 1 b k ≈ γ ^ b k + 1 b k [ 1 2 1 J b w γ δ b w ]
歐拉法給出第i時刻與第i+1時刻的預積分量估計值的關係： 【論文式[7]】α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t 2 \hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}_{i}^{b_{k}}\delta t + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t^{2} α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 2 1 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t 2 β ^ i + 1 b k = β ^ i b k + 1 2 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t \hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \frac{1}{2} R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}_{i}-b_{ai})\delta t β ^ i + 1 b k = β ^ i b k + 2 1 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 1 2 ( w ^ i − b w i ) δ t ] \hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix}
1\\
\frac{1}{2}(\hat{w}_{i}-b_{wi})\delta t
\end{bmatrix} γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 2 1 ( w ^ i − b w i ) δ t ] i i i [ t k , t k + 1 ] [t_{k}, t_{k+1}] [ t k , t k + 1 ]
代碼中採用的基於中值法的 IMU 預積分公式，這在Estimator::processIMU()函數 和IntegrationBase::push_back()函數中得以實現，注意這裏積分出來的是前後兩幀之間的 IMU 增量信息。α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 a ^ ˉ i δ t 2 \hat{\alpha}^{b_{k}}_{i+1} = \hat{\alpha}^{b_{k}}_{i} + \hat{\beta}_{i}^{b_{k}}\delta t+ \frac{1}{2} \bar{\hat{a}}_{i}\delta t^{2} α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 2 1 a ^ ˉ i δ t 2 β ^ i + 1 b k = β ^ i b k + a ^ ˉ i δ t \hat{\beta}^{b_{k}}_{i+1} = \hat{\beta}^{b_{k}}_{i} + \bar{\hat{a}}_{i}\delta t β ^ i + 1 b k = β ^ i b k + a ^ ˉ i δ t γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 1 2 w ^ ˉ i δ t ] \hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \hat{\gamma}^{i}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix}
1\\
\frac{1}{2} \bar{\hat{w}}_{i}\delta t
\end{bmatrix} γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 2 1 w ^ ˉ i δ t ] a ^ ˉ t = 1 2 [ q i ( a ^ i − b i ) + q i + 1 ( a ^ i + 1 − b a i ) ] \bar{\hat{a}}_{t} =
\frac{1}{2}[q_{i}(\hat{a}_{i}-b_{i}) + q_{i+1}(\hat{a}_{i+1}-b_{ai})] a ^ ˉ t = 2 1 [ q i ( a ^ i − b i ) + q i + 1 ( a ^ i + 1 − b a i ) ] w ^ ˉ i = 1 2 ( w ^ i + w ^ i + 1 ) − b w i \bar{\hat{w}}_{i} = \frac{1}{2}(\hat w_{i} + \hat w_{i+1})-b_{wi} w ^ ˉ i = 2 1 ( w ^ i + w ^ i + 1 ) − b w i α b k b k \alpha_{b_{k}}^{b_{k}} α b k b k β b k b k \beta_{b_{k}}^{b_{k}} β b k b k γ b k b k \gamma_{b_{k}}^{b_{k}} γ b k b k n a n_{a} n a n w n_{w} n w i i i [ k , k + 1 ] [k, k+1] [ k , k + 1 ] δ t \delta t δ t i i i i + 1 i+1 i + 1
一個 IMU 數據作爲測量值的噪聲方差我們能夠標定。現在，一段時間內多個 IMU 數據積分形成的預積分量的方差呢？
要推導預積分量的協方差，需要知道IMU噪聲和預積分量之間的線性遞推關係。
假設已知了相鄰時刻誤差的線性傳遞方程：η i k + 1 = F k η i k + G k n k \eta_{ik+1} = F_{k}\eta_{ik} + G_{k}n_{k} η i k + 1 = F k η i k + G k n k η i k \eta_{ik} η i k η i k = [ δ θ i k , δ v i k , δ p i k ] \eta_{ik}=[\delta\theta_{ik}, \delta v_{ik}, \delta p_{ik}] η i k = [ δ θ i k , δ v i k , δ p i k ] n k n_{k} n k n k = [ n k g , n k g ] n_{k}=[n_{k}^{g}, n_{k}^{g}] n k = [ n k g , n k g ] 誤差的傳遞由倆部分組成：當前時刻的誤差傳遞給下一時刻，當前時刻測量噪聲傳遞給下一時刻 。
通常對於狀態量之間的遞推關係是非線性的方程如x k + 1 = f ( x k , u k ) x_{k+1} = f(x_{k}, u_{k}) x k + 1 = f ( x k , u k ) x x x u u u
可以用倆種方法來推導狀態誤差傳遞的線性遞推關係：
一種是基於一階泰勒展開的誤差遞推方程 一種是基於誤差隨時間變化的遞推方程 (論文是基於誤差隨時間變化來推導的)
令狀態量爲x ^ = x + δ x \hat{x} = x +\delta x x ^ = x + δ x x ^ \hat{x} x ^ x x x δ x \delta x δ x u u u n n n x ^ k + 1 = f ( x ^ k , u ^ k ) \hat{x}_{k+1}=f(\hat{x}_{k},\hat{u}_{k}) x ^ k + 1 = f ( x ^ k , u ^ k ) δ x k + 1 = F δ x k + G n k \delta x_{k+1} = F\delta x_{k}+Gn_{k} δ x k + 1 = F δ x k + G n k x k + 1 x_{k+1} x k + 1 x k x_{k} x k x k + 1 x_{k+1} x k + 1 u k u_{k} u k
證明：對非線性狀態方程進行一階泰勒展開有：x ^ k + 1 = f ( x ^ k , u ^ k ) \hat{x}_{k+1} = f(\hat{x}_{k}, \hat{u}_{k}) x ^ k + 1 = f ( x ^ k , u ^ k ) x k + 1 + δ x k + 1 = f ( x k + δ x k , u k + n k ) x_{k+1}+\delta x_{k+1} = f(x_{k} + \delta x_{k}, u_{k} + n_{k}) x k + 1 + δ x k + 1 = f ( x k + δ x k , u k + n k ) x k + 1 ‾ + δ x k + 1 = f ( x k , u k ) ‾ + F δ x k + G n k (3) \underline{x_{k+1}}+\delta x_{k+1} = \underline{f(x_{k}, u_{k})} + F \delta x_{k} + Gn_{k} \tag{3} x k + 1 + δ x k + 1 = f ( x k , u k ) + F δ x k + G n k ( 3 )
如果我們能夠推導狀態誤差隨時間變化的導數關係，如：δ ˙ x = A δ x + B n \dot {\delta}_{x} = A\delta x + Bn δ ˙ x = A δ x + B n δ x k + 1 = δ x k + δ ˙ x k Δ t \delta x_{k+1} = \delta x_{k} + \dot{\delta} x_{k} \Delta t δ x k + 1 = δ x k + δ ˙ x k Δ t → δ x k + 1 = ( I + A Δ t ) δ x k + B Δ t n k (4) \rightarrow \delta x_{k+1} = (I+A\Delta t)\delta x_{k} + B\Delta tn_{k} \tag{4} → δ x k + 1 = ( I + A Δ t ) δ x k + B Δ t n k ( 4 )
由公式( 3 ) ( 4 ) (3)(4) ( 3 ) ( 4 ) F = I + A Δ t F=I+A\Delta t F = I + A Δ t G = B Δ t G=B\Delta t G = B Δ t
基於一階泰勒展開的誤差遞推方程不香嗎，爲什麼會用誤差隨時間的變化來進行誤差遞推方程的構建呢？
在VIO系統中，我們已經知道了狀態的導數和狀態之間的轉移矩陣，如我們已經知道速度和狀態量之間的關係：v ˙ w = R b w a b + g w \dot{v}^{w} = R^{w}_{b}a^{b}+g^{w} v ˙ w = R b w a b + g w v ˙ w + δ ˙ v w = R b w e x p ( [ δ θ ] × ) ( a b + δ a b ) + g + δ g \dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}exp([\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g v ˙ w + δ ˙ v w = R b w e x p ( [ δ θ ] × ) ( a b + δ a b ) + g + δ g v ˙ w + δ ˙ v w = R b w ( I + [ δ θ ] × ) ( a b + δ a b ) + g + δ g \dot{v}^{w}+\dot{\delta }v^{w} = R_{b}^{w}(I+[\delta \theta]_{\times})(a^{b} + \delta a^{b})+g+\delta g v ˙ w + δ ˙ v w = R b w ( I + [ δ θ ] × ) ( a b + δ a b ) + g + δ g v ˙ ‾ w + δ ˙ v w = R b w a b + g ‾ + R b w δ a b + R b w [ δ θ ] × ( a b + δ a b ) + δ g \underline{\dot{v}}^{w} + \dot{\delta} v^{w}= \underline{R_{b}^{w} a^{b}+ g} +R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g v ˙ w + δ ˙ v w = R b w a b + g + R b w δ a b + R b w [ δ θ ] × ( a b + δ a b ) + δ g δ ˙ v w = R b w δ a b + R b w [ δ θ ] × ( a b + δ a b ) + δ g \dot{\delta}{v}^{w} = R_{b}^{w}\delta a^{b} + R_{b}^{w} [\delta \theta]_{\times} (a^{b} + \delta a^{b}) + \delta g δ ˙ v w = R b w δ a b + R b w [ δ θ ] × ( a b + δ a b ) + δ g δ ˙ v w = R b w δ a b − R b w [ a b ] × δ θ + δ g (5) \dot{\delta} v^{w} = R_{b}^{w}\delta a^{b} - R_{b}^{w} [a^{b}]_{\times} \delta \theta + \delta g \tag{5} δ ˙ v w = R b w δ a b − R b w [ a b ] × δ θ + δ g ( 5 )
考慮到公式的編輯篇幅，爲了對一些求導公式進行簡化，對求導公式進行了簡化 (下同)： ∂ a w ∂ δ θ = l i m δ θ → 0 R b w e x p ( [ δ θ ] × ) a b − R b w a b δ θ → ∂ a w ∂ δ θ = ∂ R b w e x p ( [ δ θ ] × ) a b ∂ δ θ \frac{\partial a^{w}}{\partial \delta \theta } = \underset{\delta \theta \rightarrow 0}{lim} \frac{R_{b}^{w}exp([\delta \theta]_{\times})a^{b}-R_{b}^{w}a^{b}}{\delta \theta} \rightarrow \frac{\partial a^{w}}{\partial \delta \theta} = \frac{\partial R_{b}^{w}exp([\delta \theta]_{\times}) a^{b}}{\partial \delta \theta} ∂ δ θ ∂ a w = δ θ → 0 l im δ θ R b w e x p ( [ δ θ ] × ) a b − R b w a b → ∂ δ θ ∂ a w = ∂ δ θ ∂ R b w e x p ( [ δ θ ] × ) a b
在第4部分我們已經完成了IMU預積分測量值的推導，而要將IMU預積分運用到非線性優化中，需要建立線性高斯誤差狀態遞推方程 (可參考第5部分)，由線性高斯系統的協方差，推到方程協方差矩陣，並糾結對應的雅克比矩陣。 γ t b k \gamma_{t}^{b_{k}} γ t b k γ t b k ≈ γ ^ t b k ⊗ [ 1 1 2 δ θ t b k ] \gamma_{t}^{b_{k}} \approx \hat{\gamma}_{t}^{b_{k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \delta \theta_{t}^{b_{k}}\end{bmatrix} γ t b k ≈ γ ^ t b k ⊗ [ 1 2 1 δ θ t b k ] 一段時間內IMU構建的預積分量作爲測量值，對倆時刻之間的狀態量進行約束 ，可得到其在k k k k + 1 k+1 k + 1 [ δ α b k + 1 b k δ β b k + 1 b k δ θ b k + 1 b k δ b a δ b w ] = [ R w b k ( p b k + 1 w − p b k w − v k w Δ t + 1 2 g w Δ t 2 ) − α b k + 1 b k R w b k ( v b k + 1 w − v b k w + g w Δ t ) − β b k + 1 b k 2 [ q w b k ⊗ q b k + 1 w ⊗ γ b k b k + 1 ] x y z b a b k + 1 − b a b k b w b k + 1 − b w b k ] \begin{bmatrix} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\ \delta b_{w} \end{bmatrix} = \begin{bmatrix}
R^{b_{k}}_{w}(p^{w}_{b_{k+1}}-p_{b_{k}}^{w} - v_{k}^{w} \Delta t + \frac{1}{2}g^{w} \Delta t ^{2}) - \alpha_{b_{k+1}}^{b_{k}}\\ R^{b_{k}}_{w}(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w}\Delta t) - \beta ^{b_{k}}_{b_{k+1}} \\
2[ q_{w}^{b_{k}} \otimes q^{w}_{b_{k+1}} \otimes \gamma_{b_{k}}^{b_{k+1}} ]_{xyz} \\
b_{ab_{k+1}}-b_{ab_{k}} \\ b_{wb_{k+1}}-b_{wb_{k}}\end{bmatrix} ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ δ α b k + 1 b k δ β b k + 1 b k δ θ b k + 1 b k δ b a δ b w ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ R w b k ( p b k + 1 w − p b k w − v k w Δ t + 2 1 g w Δ t 2 ) − α b k + 1 b k R w b k ( v b k + 1 w − v b k w + g w Δ t ) − β b k + 1 b k 2 [ q w b k ⊗ q b k + 1 w ⊗ γ b k b k + 1 ] x y z b a b k + 1 − b a b k b w b k + 1 − b w b k ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
我們給出在t t t [ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ w t ] = [ 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] × − R t b k 0 0 0 − [ w ^ − b w t ] × 0 − I 0 0 0 0 0 0 0 0 0 0 ] [ δ α t b k δ β t b k δ θ t b k δ b a t δ b w t ] + [ 0 0 0 0 − R t b k 0 0 0 0 − I 0 0 0 0 I 0 0 0 0 I ] [ n a n w n b a n b w ] = F t δ z t b k + G t n t (6) \begin{bmatrix} \delta \dot{\alpha}_{t}^{b_{k}} \\ \delta \dot{\beta}_{t}^{b_{k}} \\ \delta \dot{\theta}_{t}^{b_{k}} \\ \delta \dot{b}_{at} \\ \delta \dot{b}_{wt} \end{bmatrix} = \begin{bmatrix}
0 & I & 0 & 0 & 0\\
0 & 0 & -R_{t}^{b_{k}}[\hat{a}_{t}-b_{at}]_{\times} & -R_{t}^{b_{k}} & 0\\
0 & 0 & -[\hat{w}-b_{wt}]_{\times} & 0 & -I\\
0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0
\end{bmatrix}
\begin{bmatrix} \delta \alpha_{t}^{b_{k}} \\ \delta \beta_{t}^{b_{k}} \\ \delta \theta_{t}^{b_{k}} \\ \delta b_{at} \\ \delta b_{wt} \end{bmatrix} +
\begin{bmatrix}
0 & 0 & 0 & 0\\
-R_{t}^{b_{k}} & 0 & 0 & 0\\
0& -I & 0 & 0 \\
0 & 0 & I & 0 \\
0 & 0 & 0 & I
\end{bmatrix} \begin{bmatrix} n_{a} \\ n_{w} \\ n_{ba} \\ n_{bw} \end{bmatrix} \tag{6} \\ = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t} ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ w t ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 0 0 0 0 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] × − [ w ^ − b w t ] × 0 0 0 − R t b k 0 0 0 0 0 − I 0 0 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ δ α t b k δ β t b k δ θ t b k δ b a t δ b w t ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ + ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 0 − R t b k 0 0 0 0 0 − I 0 0 0 0 0 I 0 0 0 0 0 I ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎡ n a n w n b a n b w ⎦ ⎥ ⎥ ⎤ = F t δ z t b k + G t n t ( 6 ) δ z ˙ t b k = F t δ z t b k + G t n t \delta \dot{z}_{t}^{b_{k}} = F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t} δ z ˙ t b k = F t δ z t b k + G t n t F t F_{t} F t G t G_{t} G t δ z t b k \delta z_{t}^{b_{k}} δ z t b k n t n_{t} n t δ z ˙ t b k = l i m δ t → 0 δ z t + δ t b k − δ z t b k δ t \delta \dot{z}_{t}^{b_{k}} = \underset{\delta t\rightarrow 0}{lim} \frac{\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t} δ z ˙ t b k = δ t → 0 l im δ t δ z t + δ t b k − δ z t b k δ z t + δ t b k = δ z t b k + δ z ˙ t b k δ t = δ z t b k + ( F t δ z t b k + G t n t ) δ t = ( I + F t δ t ) δ z t b k + G t δ t n t \delta z_{t+\delta t}^{b_{k}} = \delta z_{t}^{b_{k}} + \delta \dot{z}_{t}^{b_{k}} \delta t = \delta z_{t}^{b_{k}} + (F_{t}\delta z_{t}^{b_{k}} + G_{t}n_{t})\delta t = (I+F_{t} \delta t) \delta z_{t}^{b_{k}} + G_{t}\delta t n_{t} δ z t + δ t b k = δ z t b k + δ z ˙ t b k δ t = δ z t b k + ( F t δ z t b k + G t n t ) δ t = ( I + F t δ t ) δ z t b k + G t δ t n t δ t \delta t δ t Δ t \Delta t Δ t F = ( I + F t δ t ) , V = G t δ t F=(I+F_{t} \delta t), V=G_{t}\delta t F = ( I + F t δ t ) , V = G t δ t δ z t + δ t b k = F δ z t b k + V n t (7) \delta z_{t+\delta t}^{b_{k}} = F \delta z_{t}^{b_{k}} + Vn_{t} \tag{7} δ z t + δ t b k = F δ z t b k + V n t ( 7 ) 由此我們已經知道了該非線性系統的線性化遞推方程。
至於下一時刻的協方差，首先對於一個高斯分佈的變量線性變換後得到新的變量的協方差計算，我們有以下推導：y = A x y=Ax y = A x x ∼ ( 0 , Σ x ) x \sim(0, \Sigma_{x}) x ∼ ( 0 , Σ x ) Σ y = A Σ x A T \Sigma_{y} = A \Sigma_{x} A^{T} Σ y = A Σ x A T Σ y = E ( ( A x ) ( A x ) T ) = E ( A x x T A T ) = A Σ x A T \Sigma_{y} = E((Ax)(Ax)^{T}) = E(Axx^{T}A^{T}) = A \Sigma_{x} A^{T} Σ y = E ( ( A x ) ( A x ) T ) = E ( A x x T A T ) = A Σ x A T
得到了遞推方程( 7 ) (7) ( 7 ) 根據高斯分佈線性變換協方差的傳遞規律，我們可以預測下一時刻的協方差 ：【論文式(10)】P t + δ t b k = ( I + F t δ t ) P t b k ( I + F t δ t ) T + ( G δ t ) Q ( G t δ t ) T P_{t+\delta t}^{b_{k}} = (I + F_{t} \delta t) P_{t}^{b_{k}} (I + F_{t} \delta t)^{T} + (G \delta t) Q (G_{t} \delta t)^{T} P t + δ t b k = ( I + F t δ t ) P t b k ( I + F t δ t ) T + ( G δ t ) Q ( G t δ t ) T P b k b k = 0 P_{b_{k}}^{b_{k}} = 0 P b k b k = 0 Q Q Q Q 12 × 12 = [ σ a 2 0 0 0 0 σ w 2 0 0 0 0 σ b a 2 0 0 0 0 σ b w 2 ] Q^{12 \times 12} = \begin{bmatrix} \sigma _{a}^{2} & 0 & 0 &0 \\ 0 & \sigma _{w}^{2} &0 & 0 \\ 0 & 0 &\sigma_{ba}^{2} & 0 \\ 0 & 0 &0 & \sigma_{bw} ^{2} \end{bmatrix} Q 1 2 × 1 2 = ⎣ ⎢ ⎢ ⎡ σ a 2 0 0 0 0 σ w 2 0 0 0 0 σ b a 2 0 0 0 0 σ b w 2 ⎦ ⎥ ⎥ ⎤
同時也可以根據遞推方程得到對應的雅克比矩陣迭代公式：【論文式[11]】J t + δ t = ( I + F t δ t ) J t J_{t+\delta t} = (I+F_{t} \delta t) J_{t} J t + δ t = ( I + F t δ t ) J t J b k = I J_{b_{k}} = I J b k = I
關於公式( 6 ) (6) ( 6 ) α b k + 1 b k = ∫ ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 \alpha^{b_{k}}_{b_{k+1}} = \int \int _{t\in [t_{k}, t_{k+1}]} R_{t}^{b_{k}} (\hat{a}_{t}-b_{at}-n_{a}) dt^{2} α b k + 1 b k = ∫ ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t \beta_{b_{k+1}}^{b_{k}} = \int _{t\in [t_{k}, t_{k+1}]}R_{t}^{b_{k}}(\hat{a}_{t}-b_{at}-n_{a})dt β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t γ b k + 1 = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) γ t b k d t \gamma_{b_{k+1}} = \int _{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{w}_{t}-b_{wt}-n_{w}) \gamma_{t}^{b_{k}}dt γ b k + 1 = ∫ t ∈ [ t k , t k + 1 ] 2 1 Ω ( w ^ t − b w t − n w ) γ t b k d t δ α ˙ t b k \delta \dot{\alpha}_{t}^{b_{k}} δ α ˙ t b k δ b ˙ a t \delta \dot {b}_{at} δ b ˙ a t δ b ˙ w t \delta \dot{b}_{wt} δ b ˙ w t δ α ˙ t b k = α ˙ ^ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k \delta \dot{\alpha}_{t}^{b_{k}} = \hat{\dot {\alpha}}_{t}^{b_{k}} - \dot {\alpha}_{t}^{b_{k}} = \hat{\beta}_{t}^{b_{k}} - \beta_{t}^{b_{k}} = \delta \beta_{t}^{b_{k}} δ α ˙ t b k = α ˙ ^ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k δ b ˙ a t = b ˙ a t − 0 = n b a \delta \dot{b}_{at} = \dot{b}_{at} - 0 = n_{b_{a}} δ b ˙ a t = b ˙ a t − 0 = n b a δ b ˙ w t = b ˙ w t − 0 = n b w \delta \dot{b}_{wt} = \dot{b}_{wt} - 0 = n_{b_{w}} δ b ˙ w t = b ˙ w t − 0 = n b w α ˙ ^ t b k \hat{\dot {\alpha}}_{t}^{b_{k}} α ˙ ^ t b k α ˙ t b k \dot {\alpha}_{t}^{b_{k}} α ˙ t b k δ β ˙ t b k \delta \dot{\beta}_{t}^{b_{k}} δ β ˙ t b k β ˙ t b k \dot{\beta}_{t}^{b_{k}} β ˙ t b k n a n_{a} n a b b a b_{ba} b b a β ˙ t b k = R t b k ( a ^ t − b a t ) \dot{\beta}_{t}^{b_{k}}=R_{t}^{b_{k}}(\hat a_{t} - b_{at}) β ˙ t b k = R t b k ( a ^ t − b a t ) β ˙ t b k \dot \beta _{t}^{b_{k}} β ˙ t b k β ˙ ^ t b k = R ^ t b k ( a ^ t − b ^ a t − n a ) = R t b k e x p ( [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( 1 + [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( a ^ t − n a − b a t − δ b a t + [ δ θ ] × ( a ^ t − b a t ) ) = R t b k ( a ^ t − n a − b a t − δ b a t − [ a ^ t − b a t ] × δ θ ) (8) \hat{\dot{\beta}}_{t}^{b_{k}} = \hat{R}_{t}^{b_{k}} (\hat a_{t}-\hat b_{at} - n_{a}) = R_{t}^{b_{k}}exp([\delta \theta]_{\times})(\hat a_{t} - n_{a} - b_{at} - \delta b_{at}) \\ =R_{t}^{b_{k}}(1+[\delta \theta]_{\times})(\hat a_{t} - n_{a} - b_{at} - \delta b_{at}) \\ = R_{t}^{b_{k}} (\hat a_{t} - n_{a} - b_{at} - \delta b_{at} + [\delta \theta]_{\times} (\hat a_{t} - b_{at})) \\ = R_{t}^{b_{k}} (\hat a_{t} - n_{a} - b_{at} - \delta b_{at} - [\hat a_{t} - b_{at}]_{\times} \delta \theta ) \tag{8} β ˙ ^ t b k = R ^ t b k ( a ^ t − b ^ a t − n a ) = R t b k e x p ( [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( 1 + [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) = R t b k ( a ^ t − n a − b a t − δ b a t + [ δ θ ] × ( a ^ t − b a t ) ) = R t b k ( a ^ t − n a − b a t − δ b a t − [ a ^ t − b a t ] × δ θ ) ( 8 ) δ β ˙ t b k = β ˙ ^ t b k − β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a \delta \dot{\beta}_{t}^{b_{k}} = \hat{\dot{\beta}}_{t}^{b_{k}} - \dot{\beta}_{t}^{b_{k}} = -R_{t}^{b_{k}} [\hat a_{t} - b_{at}]_{\times} \delta \theta - R_{t}^{b_{k}}\delta b_{at} - R_{t}^{b_{k}}n_{a} δ β ˙ t b k = β ˙ ^ t b k − β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a ( 8 ) (8) ( 8 ) ( 5 ) (5) ( 5 ) δ θ ˙ t b k \delta \dot{\theta} _{t}^{b_{k}} δ θ ˙ t b k δ q ˙ t b k \delta \dot{q}_{t}^{b_{k}} δ q ˙ t b k q ˙ t b k \dot q_{t}^{b_{k}} q ˙ t b k n w n_{w} n w n b w n_{b_{w}} n b w q ˙ t b k = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] = 1 2 Ω ( w ^ t − b w t ) q t b k \dot q_{t}^{b_{k}} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t} - b_{wt}) \\ 0 \end{bmatrix}= \frac{1}{2} \Omega(\hat{w}_{t} - b_{wt}) q_{t}^{b_{k}} q ˙ t b k = 2 1 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] = 2 1 Ω ( w ^ t − b w t ) q t b k q ˙ t b k \dot q_{t}^{b_{k}} q ˙ t b k q ˙ t ^ b k = 1 2 q t ^ b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] = 1 2 q t b k ⊗ δ q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] \hat{\dot q_{t}}^{b_{k}} = \frac{1}{2} \hat{q_{t}}^{b_{k}} \otimes \begin{bmatrix} (\hat{w}_{t} - n_{w} - b_{wt} - \delta b_{wt}) \\ 0 \end{bmatrix} \\ = \frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} - n_{w} - b_{wt} - \delta b_{wt}) \\ 0 \end{bmatrix} q ˙ t ^ b k = 2 1 q t ^ b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] = 2 1 q t b k ⊗ δ q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] q ˙ t ^ b k = ( q t b k ⊗ ˙ δ q t b k ) = q ˙ t b k ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k \hat{\dot q_{t}}^{b_{k}} = (q_{t}^{b_{k}} \dot \otimes \delta q_{t}^{b_{k}}) = \dot q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat {w}_{t} - b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}} q ˙ t ^ b k = ( q t b k ⊗ ˙ δ q t b k ) = q ˙ t b k ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k = 2 1 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k 1 2 q t b k ⊗ δ q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k \frac{1}{2} q_{t}^{b_{k}} \otimes \delta q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} - n_{w} - b_{wt} - \delta b_{wt}) \\ 0 \end{bmatrix} = \frac{1}{2} q_{t}^{b_{k}} \otimes \begin{bmatrix} (\hat {w}_{t} - b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}} + q_{t}^{b_{k}} \otimes \delta \dot q_{t}^{b_{k}} 2 1 q t b k ⊗ δ q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] = 2 1 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q ˙ t b k 2 δ q ˙ t b k = q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] − [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k 2 \delta \dot q_{t}^{b_{k}} = q_{t}^{b_{k}}\otimes \begin{bmatrix} (\hat{w}_{t} - n_{w} - b_{wt} - \delta b_{wt}) \\ 0 \end{bmatrix} - \begin{bmatrix} (\hat {w}_{t} - b_{wt}) \\ 0 \end{bmatrix} \otimes \delta q_{t}^{b_{k}} 2 δ q ˙ t b k = q t b k ⊗ [ ( w ^ t − n w − b w t − δ b w t ) 0 ] − [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k 2 δ q ˙ t b k = R ( [ ( w ^ t − n w − b w t − δ b w t ) 0 ] ) δ q t b k − L ( [ ( w ^ t − b w t ) 0 ] ) δ q t b k 2 \delta \dot q_{t}^{b_{k}} = R(\begin{bmatrix} (\hat{w}_{t} - n_{w} - b_{wt} - \delta b_{wt}) \\ 0 \end{bmatrix} ) \delta q_{t}^{b_{k}}- L(\begin{bmatrix} (\hat {w}_{t} - b_{wt}) \\ 0 \end{bmatrix}) \delta q_{t}^{b_{k}} 2 δ q ˙ t b k = R ( [ ( w ^ t − n w − b w t − δ b w t ) 0 ] ) δ q t b k − L ( [ ( w ^ t − b w t ) 0 ] ) δ q t b k 2 δ q ˙ t b k = [ δ θ ˙ t b k 0 ] = [ − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × − n w − δ b w t ( n w + δ b w t ) T 0 ] [ 1 2 δ θ t b k 1 ] 2 \delta \dot q_{t}^{b_{k}} = \begin{bmatrix} \delta \dot{\theta}_{t}^{b_{k}} \\ 0\end{bmatrix}= \begin{bmatrix} -[2\hat{w}_{t} - 2b_{wt} - n_{w} - \delta b_{wt}]_{\times} & -n_{w}-\delta b_{wt}\\ (n_{w}+\delta b_{wt})^{T} & 0\end{bmatrix} \begin{bmatrix} \frac{1}{2} \delta \theta _{t}^{b_{k}} \\ 1 \end{bmatrix} 2 δ q ˙ t b k = [ δ θ ˙ t b k 0 ] = [ − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × ( n w + δ b w t ) T − n w − δ b w t 0 ] [ 2 1 δ θ t b k 1 ] δ θ ˙ t b k = − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × 1 2 δ θ t b k − n w − δ b w t ≈ − [ w ^ t − b w t ] × δ θ t b k − n w − δ b w t \delta \dot{\theta}_{t}^{b_{k}} =-[2\hat{w}_{t} - 2b_{wt} - n_{w} - \delta b_{wt}]_{\times} \frac{1}{2} \delta \theta _{t}^{b_{k}} - n_{w} - \delta b_{wt} \\ \approx -[\hat{w}_{t} - b_{wt} ]_{\times} \delta \theta _{t}^{b_{k}} - n_{w} - \delta b_{wt} δ θ ˙ t b k = − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × 2 1 δ θ t b k − n w − δ b w t ≈ − [ w ^ t − b w t ] × δ θ t b k − n w − δ b w t
根據連續形式的增量誤差遞推方程，同理我們可以推導出離散形式的方程，首先直接給出 PVQ 增量誤差在離散形式下的矩陣形式，這在vins_estimator/src/factor/integration_base.h中的void integrationBase::midPointIntegration()函數中得以實現 (這裏的矩陣V幾個變量與VINS中的代碼差個負號，體現在前四個噪聲項，應該是推導過程中與VINS添加噪聲的方式不同，即是減去一個噪聲還是加上一個噪聲，問題不是特別大)[ δ α k + 1 δ θ k + 1 δ β k + 1 δ b a k + 1 δ b w k + 1 ] = [ I f 01 δ t f 03 f 04 0 f 11 0 0 − δ t 0 f 21 I f 23 f 24 0 0 0 I 0 0 0 0 0 I ] [ δ α k δ θ k δ β k δ b a k δ b w k ] + [ v 00 v 01 v 02 v 03 0 0 0 − 1 2 δ t 0 − 1 2 δ t 0 0 − 1 2 R k δ t v 21 − 1 2 R k + 1 δ t v 23 0 0 0 0 0 0 δ t 0 0 0 0 0 0 δ t ] [ n a k n w k n a k + 1 n w k + 1 n b a n b w ] (9) \begin{bmatrix} \delta \alpha_{k+1} \\ \delta \theta_{k+1} \\ \delta \beta_{k+1} \\ \delta b_{ak+1} \\ \delta b_{wk+1} \end{bmatrix} = \begin{bmatrix} I & f_{01} & \delta t & f_{03} & f_{04} \\ 0 & f_{11} & 0 & 0 & -\delta t \\ 0 & f_{21} & I & f_{23} & f_{24} \\ 0 & 0& 0 & I & 0 \\ 0 & 0& 0& 0 & I\end{bmatrix} \begin{bmatrix} \delta \alpha_{k} \\ \delta \theta_{k} \\ \delta \beta_{k} \\ \delta b_{ak} \\ \delta b_{wk} \end{bmatrix} +
\begin{bmatrix} v_{00} & v_{01} &v_{02} & v_{03} & 0 & 0 \\ 0 & -\frac{1}{2} \delta t & 0 & -\frac{1}{2} \delta t& 0 & 0 \\ -\frac{1}{2}R_{k}\delta t & v_{21} & -\frac{1}{2}R_{k+1}\delta t & v_{23} & 0 & 0\\ 0 & 0& 0 & 0 & \delta t&0 \\ 0 & 0& 0& 0 & 0 & \delta t\end{bmatrix} \begin{bmatrix} n_{ak} \\ n_{wk} \\ n_{ak+1} \\ n_{wk+1} \\ n_{ba} \\ n_{bw}\end{bmatrix} \tag{9} ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ δ α k + 1 δ θ k + 1 δ β k + 1 δ b a k + 1 δ b w k + 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ I 0 0 0 0 f 0 1 f 1 1 f 2 1 0 0 δ t 0 I 0 0 f 0 3 0 f 2 3 I 0 f 0 4 − δ t f 2 4 0 I ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ δ α k δ θ k δ β k δ b a k δ b w k ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ + ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ v 0 0 0 − 2 1 R k δ t 0 0 v 0 1 − 2 1 δ t v 2 1 0 0 v 0 2 0 − 2 1 R k + 1 δ t 0 0 v 0 3 − 2 1 δ t v 2 3 0 0 0 0 0 δ t 0 0 0 0 0 δ t ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ n a k n w k n a k + 1 n w k + 1 n b a n b w ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ ( 9 ) f 01 = 1 2 f 21 δ t , f 03 = − 1 4 ( R k + R k + 1 ) δ t 2 , f 04 = 1 2 f 24 δ t f_{01} = \frac{1}{2} f_{21}\delta t, \ f_{03} = -\frac{1}{4}(R_{k}+R_{k+1})\delta t^{2}, \ f_{04}=\frac{1}{2} f_{24} \delta {t} f 0 1 = 2 1 f 2 1 δ t , f 0 3 = − 4 1 ( R k + R k + 1 ) δ t 2 , f 0 4 = 2 1 f 2 4 δ t f 11 = I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t f_{11}=I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t f 1 1 = I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t f 21 = − 1 2 R k [ a ^ k − b a k ] × δ t − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) δ t f_{21} = -\frac{1}{2}R_{k} [\hat a_{k} - b_{ak}]_{\times} \delta t -\frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t) \delta t f 2 1 = − 2 1 R k [ a ^ k − b a k ] × δ t − 2 1 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) δ t f 23 = − 1 2 ( R k + R k + 1 ) δ t , f 24 = 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 f_{23} = -\frac{1}{2}(R_{k} + R_{k+1})\delta t, \ f_{24} = \frac{1}{2} R_{k+1}[\hat {a}_{k+1} - b_{a_{k}}]_{\times} \delta t^ {2} f 2 3 = − 2 1 ( R k + R k + 1 ) δ t , f 2 4 = 2 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 v 00 = − 1 4 R k δ t 2 , v 01 = v 03 = 1 2 v 21 δ t , v 02 = − 1 4 R k + 1 δ t 2 v_{00}=-\frac{1}{4}R_{k}\delta t^{2}, \ v_{01} = v_{03} = \frac{1}{2} v_{21}\delta t, \ v_{02} = -\frac{1}{4}R_{k+1}\delta t^{2} v 0 0 = − 4 1 R k δ t 2 , v 0 1 = v 0 3 = 2 1 v 2 1 δ t , v 0 2 = − 4 1 R k + 1 δ t 2 v 21 = v 23 = 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 v_{21}=v_{23} = \frac{1}{4}R_{k+1}[\hat{a}_{k+1}-b_{ak}]_{\times} \delta t^{2} v 2 1 = v 2 3 = 4 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 δ z k + 1 15 × 1 = F 15 × 15 δ z k 15 × 1 + V 15 × 18 n 18 × 1 (10) \delta z_{k+1}^{15 \times 1} = F^{15 \times 15} \delta z_{k}^{15 \times 1} + V^{15 \times 18} n^{18 \times 1} \tag{10} δ z k + 1 1 5 × 1 = F 1 5 × 1 5 δ z k 1 5 × 1 + V 1 5 × 1 8 n 1 8 × 1 ( 1 0 ) 協方差的迭代公式爲（協方差的初始值P 0 = 0 P_{0} = 0 P 0 = 0 ：P k + 1 = F P k F T + V Q V T P_{k+1} = FP_{k}F^{T} + VQV^{T} P k + 1 = F P k F T + V Q V T Q Q Q Q 18 × 18 = ( σ a 2 , σ w 2 , σ a 2 , σ w 2 , σ b a 2 , σ b w 2 ) Q^{18 \times 18} = (\sigma_{a}^{2}, \sigma_{w}^{2},\sigma_{a}^{2},\sigma_{w}^{2},\sigma_{ba}^{2},\sigma_{bw}^{2}) Q 1 8 × 1 8 = ( σ a 2 , σ w 2 , σ a 2 , σ w 2 , σ b a 2 , σ b w 2 ) 雅克比矩陣的迭代公式爲（雅克比矩陣的初始值爲J 0 = I J_{0} =I J 0 = I J k + 1 = F J k J_{k+1} = FJ_{k} J k + 1 = F J k J k + 1 J_{k+1} J k + 1
關於公式(9)的推導：δ b a k + 1 \delta b_{ak+1} δ b a k + 1 δ b w k + 1 \delta b_{wk+1} δ b w k + 1 δ b a k + 1 = δ b a k + n b a δ t , δ b w k + 1 = δ b w k + n b w δ t \delta b_{ak+1} = \delta b_{ak} + n_{b_{a}} \delta t, \ \delta b_{wk+1} = \delta b_{wk} + n_{b_{w}} \delta t δ b a k + 1 = δ b a k + n b a δ t , δ b w k + 1 = δ b w k + n b w δ t δ θ k + 1 \delta \theta_{k+1} δ θ k + 1 δ θ ˙ t b k = − [ w ^ t − b w t ] × δ θ t b k − n w − δ b w t \delta \dot{\theta}_{t}^{b_{k}} = -[\hat{w}_{t} - b_{wt}]_{\times} \delta \theta_{t}^{b_{k}} - n_{w} - \delta b_{wt} δ θ ˙ t b k = − [ w ^ t − b w t ] × δ θ t b k − n w − δ b w t δ θ ˙ k = − [ w ^ k + w ^ k + 1 2 − b w k ] × δ θ k b k − n w k + n w k + 1 2 − δ b w k \delta \dot{\theta}_{k} = -[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta \theta_{k}^{b_{k}} - \frac{n_{wk}+n_{wk+1}}{2} - \delta b_{wk} δ θ ˙ k = − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ θ k b k − 2 n w k + n w k + 1 − δ b w k δ θ k + 1 = δ θ k + δ θ ˙ k δ t = ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) δ θ k − n w k + n w k + 1 2 δ t − δ b w k δ t \delta \theta_{k+1} = \delta \theta_{k} + \delta \dot{\theta}_{k} \delta t = (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t) \delta \theta_{k} - \frac{n_{wk}+n_{wk+1}}{2} \delta t- \delta b_{wk} \delta t δ θ k + 1 = δ θ k + δ θ ˙ k δ t = ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) δ θ k − 2 n w k + n w k + 1 δ t − δ b w k δ t δ θ k + 1 = ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) δ θ k − δ t δ b w k − δ t 2 n w k − δ t 2 n w k + 1 = f 11 δ θ k − δ t δ b w k − δ t 2 n w k − δ t 2 n w k + 1 \delta \theta_{k+1} = (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t) \delta \theta_{k} - \delta t \delta b_{wk} - \frac{\delta t}{2} n_{wk} - \frac{\delta t}{2} n_{wk+1} = f_{11} \delta \theta_{k} - \delta t \delta b_{wk} - \frac{\delta t}{2} n_{wk} - \frac{\delta t}{2} n_{wk+1} δ θ k + 1 = ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) δ θ k − δ t δ b w k − 2 δ t n w k − 2 δ t n w k + 1 = f 1 1 δ θ k − δ t δ b w k − 2 δ t n w k − 2 δ t n w k + 1 f 11 = I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t f_{11}=I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t f 1 1 = I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t δ β k + 1 : \delta \beta_{k+1}: δ β k + 1 : δ β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a \delta \dot{\beta}_{t}^{b_{k}} = -R_{t}^{b_{k}} [\hat a_{t} - b_{at}]_{\times} \delta \theta - R_{t}^{b_{k}}\delta b_{at} - R_{t}^{b_{k}}n_{a} δ β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a δ β ˙ k = − 1 2 R k [ a ^ k − b a k ] × δ θ k − 1 2 R k + 1 [ a ^ k + 1 − b a k + 1 ] × δ θ k + 1 − 1 2 ( R k + R k + 1 ) δ b a k − 1 2 R k n a k − 1 2 R k + 1 n a k + 1 \delta \dot{\beta}_{k} = -\frac{1}{2}R_{k} [\hat a_{k} - b_{ak}]_{\times} \delta \theta_{k} -\frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak+1}]_{\times}\delta \theta_{k+1} - \frac{1}{2}(R_{k}+R_{k+1}) \delta b_{ak} -\frac{1}{2} R_{k}n_{ak} - \frac{1}{2} R_{k+1}n_{ak+1} δ β ˙ k = − 2 1 R k [ a ^ k − b a k ] × δ θ k − 2 1 R k + 1 [ a ^ k + 1 − b a k + 1 ] × δ θ k + 1 − 2 1 ( R k + R k + 1 ) δ b a k − 2 1 R k n a k − 2 1 R k + 1 n a k + 1 k k k k + 1 k+1 k + 1 b a b_{a} b a δ θ k + 1 \delta \theta_{k+1} δ θ k + 1 δ β ˙ k = { − 1 2 R k [ a ^ k − b a k ] × − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) } δ θ k + 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 1 2 ( R k + R k + 1 ) δ b a k − 1 2 R k n a k − 1 2 R k + 1 n a k + 1 \delta \dot{\beta}_{k} = \begin{Bmatrix} -\frac{1}{2}R_{k} [\hat a_{k} - b_{ak}]_{\times} -\frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t)\end{Bmatrix} \delta \theta_{k} \\ + \frac{1}{4} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times}\delta t (n_{wk}+n_{wk+1}) + \frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} \delta t \delta b_{wk} \\ -\frac{1}{2}(R_{k} + R_{k+1})\delta b_{ak} - \frac{1}{2} R_{k}{n_{ak}} - \frac{1}{2}R_{k+1}{n_{ak+1}} δ β ˙ k = { − 2 1 R k [ a ^ k − b a k ] × − 2 1 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) } δ θ k + 4 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 2 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 2 1 ( R k + R k + 1 ) δ b a k − 2 1 R k n a k − 2 1 R k + 1 n a k + 1 δ β k + 1 = δ β k + δ β ˙ k δ t = { − 1 2 R k [ a ^ k − b a k ] × − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) } δ t δ θ k + δ β k + { 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 1 2 ( R k + R k + 1 ) δ b a k − 1 2 R k n a k − 1 2 R k + 1 n a k + 1 } δ t \delta \beta_{k+1} = \delta \beta_{k} + \delta \dot {\beta}_{k} \delta t \\ = \begin{Bmatrix} -\frac{1}{2}R_{k} [\hat a_{k} - b_{ak}]_{\times} -\frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t)\end{Bmatrix} \delta t \delta \theta_{k} + \delta \beta _{k} + \\ \begin{Bmatrix} \frac{1}{4} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times}\delta t (n_{wk}+n_{wk+1}) + \frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} \delta t \delta b_{wk} -\frac{1}{2}(R_{k} + R_{k+1})\delta b_{ak} - \frac{1}{2} R_{k}{n_{ak}} - \frac{1}{2}R_{k+1}{n_{ak+1}} \end{Bmatrix} \delta t δ β k + 1 = δ β k + δ β ˙ k δ t = { − 2 1 R k [ a ^ k − b a k ] × − 2 1 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) } δ t δ θ k + δ β k + { 4 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t ( n w k + n w k + 1 ) + 2 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t δ b w k − 2 1 ( R k + R k + 1 ) δ b a k − 2 1 R k n a k − 2 1 R k + 1 n a k + 1 } δ t δ β k + 1 = f 21 δ θ k + δ β k + f 23 δ b a k + f 24 δ b w k − 1 2 R k δ t n a k − 1 2 R k + 1 δ t n a k + 1 + v 21 n w k + v 23 n w k + 1 \delta \beta_{k+1} = f_{21} \delta \theta_{k} + \delta \beta _{k} + f_{23}\delta b_{ak} + f_{24}\delta b_{wk} - \frac{1}{2} R_{k}\delta tn_{ak} \\ - \frac{1}{2}R_{k+1}\delta tn_{ak+1} + v_{21}n_{wk}+v_{23}n_{wk+1} δ β k + 1 = f 2 1 δ θ k + δ β k + f 2 3 δ b a k + f 2 4 δ b w k − 2 1 R k δ t n a k − 2 1 R k + 1 δ t n a k + 1 + v 2 1 n w k + v 2 3 n w k + 1 f 21 = − 1 2 R k [ a ^ k − b a k ] × δ t − 1 2 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ w ^ k + w ^ k + 1 2 − b w k ] × δ t ) δ t f_{21} = -\frac{1}{2}R_{k} [\hat a_{k} - b_{ak}]_{\times} \delta t -\frac{1}{2} R_{k+1} [\hat a_{k+1} - b_{ak}]_{\times} (I-[\frac{\hat{w}_{k}+\hat{w}_{k+1}}{2} - b_{wk}]_{\times} \delta t) \delta t f 2 1 = − 2 1 R k [ a ^ k − b a k ] × δ t − 2 1 R k + 1 [ a ^ k + 1 − b a k ] × ( I − [ 2 w ^ k + w ^ k + 1 − b w k ] × δ t ) δ t f 23 = − 1 2 ( R k + R k + 1 ) δ t f_{23} = -\frac{1}{2}(R_{k} + R_{k+1})\delta t f 2 3 = − 2 1 ( R k + R k + 1 ) δ t f 24 = 1 2 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 f_{24} = \frac{1}{2} R_{k+1}[\hat {a}_{k+1} - b_{a_{k}}]_{\times} \delta t^ {2} f 2 4 = 2 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 v 21 = v 23 = 1 4 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 v_{21}=v_{23} = \frac{1}{4}R_{k+1}[\hat{a}_{k+1}-b_{ak}]_{\times} \delta t^{2} v 2 1 = v 2 3 = 4 1 R k + 1 [ a ^ k + 1 − b a k ] × δ t 2 δ α k + 1 : \delta \alpha_{k+1}: δ α k + 1 : δ α ˙ t b k = α ˙ ^ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k \delta \dot{\alpha}_{t}^{b_{k}} = \hat{\dot {\alpha}}_{t}^{b_{k}} - \dot {\alpha}_{t}^{b_{k}} = \hat{\beta}_{t}^{b_{k}} - \beta_{t}^{b_{k}} = \delta \beta_{t}^{b_{k}} δ α ˙ t b k = α ˙ ^ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k δ α ˙ t b k = 1 2 δ β k + 1 2 δ β k + 1 = δ β k + 1 2 f 21 δ θ k − 1 4 ( R k + R k + 1 ) δ t δ b a k + 1 2 f 24 δ b w k − 1 4 R k δ t n a k − 1 4 R k + 1 δ t n a k + 1 + 1 2 v 21 n w k + 1 2 v 23 n w k + 1 \delta \dot{\alpha}_{t}^{b_{k}} = \frac{1}{2}\delta \beta_{k} + \frac{1}{2} \delta \beta_{k+1} \\ = \delta \beta_{k} + \frac{1}{2} f_{21}\delta \theta_{k} - \frac{1}{4}(R_{k}+R_{k+1})\delta t \delta b_{ak} + \frac{1}{2} f_{24} \delta b_{wk} \\ - \frac{1}{4} R_{k}\delta t n_{ak} - \frac{1}{4}R_{k+1}\delta t n_{ak+1} + \frac{1}{2} v_{21}n_{wk} + \frac{1}{2} v_{23}n_{wk+1} δ α ˙ t b k = 2 1 δ β k + 2 1 δ β k + 1 = δ β k + 2 1 f 2 1 δ θ k − 4 1 ( R k + R k + 1 ) δ t δ b a k + 2 1 f 2 4 δ b w k − 4 1 R k δ t n a k − 4 1 R k + 1 δ t n a k + 1 + 2 1 v 2 1 n w k + 2 1 v 2 3 n w k + 1 δ α k + 1 = δ α k + δ α ˙ δ t = δ α k + f 01 δ θ k + δ t δ β k + f 03 δ b a k + f 04 δ b w k + v 00 n a k + v 01 n w k + v 02 n a k + 1 + v 03 n w k + 1 \delta \alpha_{k+1} = \delta{\alpha}_{k} + \delta \dot {\alpha} \delta t \\ = \delta \alpha _{k} + f_{01} \delta \theta_{k} + \delta t \delta \beta _{k} + f_{03}\delta b_{ak} + f_{04} \delta b_{wk} + v_{00}n_{ak} + v_{01}n_{wk}+v_{02}n_{ak+1}+v_{03}n_{wk+1} δ α k + 1 = δ α k + δ α ˙ δ t = δ α k + f 0 1 δ θ k + δ t δ β k + f 0 3 δ b a k + f 0 4 δ b w k + v 0 0 n a k + v 0 1 n w k + v 0 2 n a k + 1 + v 0 3 n w k + 1 f 01 = 1 2 f 21 δ t , f 03 = − 1 4 ( R k + R k + 1 ) δ t 2 , f 04 = 1 2 f 24 δ t f_{01} = \frac{1}{2} f_{21} \delta t, \ f_{03} = -\frac{1}{4}(R_{k}+R_{k+1})\delta t^{2}, \ f_{04}=\frac{1}{2} f_{24} \delta {t} f 0 1 = 2 1 f 2 1 δ t , f 0 3 = − 4 1 ( R k + R k + 1 ) δ t 2 , f 0 4 = 2 1 f 2 4 δ t v 00 = − 1 4 R k δ t 2 , v 01 = v 03 = 1 2 v 21 δ t , v 02 = − 1 4 R k + 1 δ t 2 v_{00}=-\frac{1}{4}R_{k}\delta t^{2}, \ v_{01} = v_{03} = \frac{1}{2} v_{21}\delta t, \ v_{02} = -\frac{1}{4}R_{k+1}\delta t^{2} v 0 0 = − 4 1 R k δ t 2 , v 0 1 = v 0 3 = 2 1 v 2 1 δ t , v 0 2 = − 4 1 R k + 1 δ t 2
