IMU预积分原理

PVQ增量预积分(可理解为EKF过程中的状态估计)

PVQ增量的连续形式
参照VINS-Mono论文知识点精炼(翻译)中IMU预积分章节，可知PVQ增量连续时间积分公式如下:
α b k + 1 b k = ∬ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t ) d t 2 β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t ) d t γ b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( ω ^ t − b w t ) γ t b k d t (1) \begin{aligned} &\boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} = \iint_{t\in[t_{k},t_{k+1}]}\mathbf{R}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{a_{t}})dt^{2} \\ &\boldsymbol{\beta}_{b_{k+1}}^{b_{k}} = \int_{t\in[t_{k},t_{k+1}]}\mathbf{R}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \mathbf{b}_{a_{t}})dt \\ &\boldsymbol{\gamma}_{b_{k+1}^{b_{k}}} = \int_{t\in[t_{k},t_{k+1}]}\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}}_{t}-\mathbf{b}_{w_{t}})\boldsymbol{\gamma}_{t}^{b_{k}} dt \end{aligned} \tag{1} αbk+1bk=∬t∈[tk,tk+1]Rtbk(a^t−bat)dt2βbk+1bk=∫t∈[tk,tk+1]Rtbk(a^t−bat)dtγbk+1bk=∫t∈[tk,tk+1]21Ω(ω^t−bwt)γtbkdt(1)
其中:
Ω ( ω ) = [ − ⌊ ω ⌋ × ω − ω T 0 ] , ⌊ ω ⌋ × = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] (2) \boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} -\left \lfloor \boldsymbol{\omega} \right \rfloor_{\times} & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^{T} & 0 \end{bmatrix}, \left\lfloor \boldsymbol{\omega} \right\rfloor_{\times}= \begin{bmatrix} 0 & -\boldsymbol{\omega}_{z} & \boldsymbol{\omega}_{y} \\ \boldsymbol{\omega}_{z} & 0 & -\boldsymbol{\omega}_{x} \\ -\boldsymbol{\omega}_{y} & \boldsymbol{\omega}_{x} & 0 \end{bmatrix} \tag{2} Ω(ω)=[−⌊ω⌋×−ωTω0],⌊ω⌋×=⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤(2)
相邻两IMU帧之间PVQ增量的离散形式(中值积分)
注:论文中推导用的是欧拉法积分，而代码中用的是中值积分，因此为了与代码保持一致，此处写出的是中值积分形式．
α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 a ˉ ^ i δ t 2 β ^ i + 1 b k = β ^ i b k + a ˉ ^ i δ t γ ^ i + 1 b k = γ ^ i b k ⊗ [ 1 1 2 ω ˉ ^ i δ t ] (3) \begin{aligned} \hat{\boldsymbol{\alpha}}_{i+1}^{b_{k}} &= \hat{\boldsymbol{\alpha}}_{i}^{b_{k}} + \hat{\boldsymbol{\beta}}_{i}^{b_{k}}\delta t + \frac{1}{2}\hat{\bar{\mathbf{a}}}_{i}\delta t^{2} \\ \hat{\boldsymbol{\beta}}_{i+1}^{b_{k}} &= \hat{\boldsymbol{\beta}}_{i}^{b_{k}} + \hat{\bar{\mathbf{a}}}_{i}\delta t \\ \hat{\boldsymbol{\gamma}}_{i+1}^{b_{k}} &= \hat{\boldsymbol{\gamma}}_{i}^{b_{k}}\otimes\begin{bmatrix} 1 \\ \frac{1}{2}\hat{\boldsymbol{\bar{\omega}}}_{i}\delta t \end{bmatrix} \end{aligned} \tag{3} α^i+1bkβ^i+1bkγ^i+1bk=α^ibk+β^ibkδt+21aˉ^iδt2=β^ibk+aˉ^iδt=γ^ibk⊗[121ωˉ^iδt](3)
式中 ( a ˉ ^ i , ω ˉ ^ i ) (\hat{\bar{\mathbf{a}}}_{i},\hat{\boldsymbol{\bar{\omega}}}_{i}) (aˉ^i,ωˉ^i)分别是相邻两帧测量值的平均值，计算如下:
a ˉ ^ i = 1 2 ( R ( q i ) ( a ^ i − b a i ) − R ( q i + 1 ) ( a ^ i + 1 − b a i + 1 ) ) ω ˉ ^ i = 1 2 ( ω ^ i + 1 − b w i + 1 + ω ^ i − b w i ) (4) \begin{aligned} \hat{\bar{\mathbf{a}}}_{i} &= \frac{1}{2}(\mathbf{R}(\mathbf{q}_{i})(\hat{\mathbf{a}}_{i}-\mathbf{b}_{a_{i}}) - \mathbf{R}(\mathbf{q_{i+1}})(\hat{\mathbf{a}}_{i+1}-\mathbf{b}_{a_{i+1}})) \\ \hat{\boldsymbol{\bar{\omega}}}_{i} &= \frac{1}{2}(\hat{\boldsymbol{\omega}}_{i+1} - \mathbf{b}_{w_{i+1}} + \hat{\boldsymbol{\omega}}_{i} - \mathbf{b}_{w_{i}}) \end{aligned} \tag{4} aˉ^iωˉ^i=21(R(qi)(a^i−bai)−R(qi+1)(a^i+1−bai+1))=21(ω^i+1−bwi+1+ω^i−bwi)(4)

误差传递(可理解为EKF过程中的协方差估计)

实际PVQ增量的导数
α ˙ t b k = β t b k β ˙ t b k = R t b k ( a t − b a t ) γ ˙ t b k = 1 2 γ t b k ⊗ [ 0 ω t − b w t ] b ˙ a t = n b a b ˙ w t = n b w (5) \begin{aligned} \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} &= \boldsymbol{\beta}_{t}^{b_{k}}\\ \dot{\boldsymbol{\beta}}_{t}^{b_{k}} &= \mathbf{R}_{t}^{b_{k}}(\mathbf{a}_{t} - \mathbf{b}_{a_{t}})\\ \dot{\boldsymbol{\gamma}}_{t}^{b_{k}} &= \frac{1}{2}\boldsymbol{\gamma}_{t}^{b_{k}}\otimes\begin{bmatrix} 0 \\ \boldsymbol{\omega}_{t} - \mathbf{b}_{w_{t}} \end{bmatrix}\\ \dot{\mathbf{b}}_{a_{t}} &= \mathbf{n}_{b_{a}}\\ \dot{\mathbf{b}}_{w_{t}} &= \mathbf{n}_{b_{w}} \end{aligned} \tag{5} α˙tbkβ˙tbkγ˙tbkb˙atb˙wt=βtbk=Rtbk(at−bat)=21γtbk⊗[0ωt−bwt]=nba=nbw(5)
名义PVQ增量的导数
α ^ ˙ t b k = β ^ t b k β ^ ˙ t b k = R ^ t b k ( a ^ t − b ^ a t ) γ ^ ˙ t b k = 1 2 γ ^ t b k ⊗ [ 0 ω ^ t − b ^ w t ] b ^ ˙ a t = 0 3 × 1 b ^ ˙ w t = 0 3 × 1 (6) \begin{aligned} \dot{\hat{\boldsymbol{\alpha}}}_{t}^{b_{k}} &= \hat{\boldsymbol{\beta}}_{t}^{b_{k}}\\ \dot{\hat{\boldsymbol{\beta}}}_{t}^{b_{k}} &= \hat{\mathbf{R}}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}})\\ \dot{\hat{\boldsymbol{\gamma}}}_{t}^{b_{k}} &= \frac{1}{2}\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} - \hat{\mathbf{b}}_{w_{t}} \end{bmatrix}\\ \dot{\hat{\mathbf{b}}}_{a_{t}} &= \mathbf{0}_{3\times1}\\ \dot{\hat{\mathbf{b}}}_{w_{t}} &= \mathbf{0}_{3\times1} \end{aligned} \tag{6} α^˙tbkβ^˙tbkγ^˙tbkb^˙atb^˙wt=β^tbk=R^tbk(a^t−b^at)=21γ^tbk⊗[0ω^t−b^wt]=03×1=03×1(6)
PVQ增量误差的导数
先直接写出PVQ增量误差的导数结果:
[ δ α ˙ 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 − ⌊ ω ^ t − 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 (7) \begin{aligned} \begin{bmatrix} \delta \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} \\ \delta \dot{\boldsymbol{\beta}}_{t}^{b_{k}} \\ \delta \dot{\boldsymbol{\theta}}_{t}^{b_{k}} \\ \delta \dot{\mathbf{b}}_{a_{t}} \\ \delta \dot{\mathbf{b}}_{w_{t}} \end{bmatrix} &= \begin{bmatrix} 0 & \mathbf{I} & 0 & 0 & 0 \\ 0 & 0 & -\mathbf{R}_{t}^{b_{k}}\left \lfloor \hat{\mathbf{a}}_{t} - \mathbf{b}_{a_{t}} \right \rfloor_{\times} & -\mathbf{R}_{t}^{b_{k}} & 0 \\ 0 & 0 & -\left\lfloor \hat{\boldsymbol{\omega}}_{t}-\mathbf{b}_{w_{t}} \right\rfloor_{\times} & 0 & -\mathbf{I} \\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \delta {\boldsymbol{\alpha}}_{t}^{b_{k}} \\ \delta {\boldsymbol{\beta}}_{t}^{b_{k}} \\ \delta {\boldsymbol{\theta}}_{t}^{b_{k}} \\ \delta {\mathbf{b}}_{a_{t}} \\ \delta {\mathbf{b}}_{w_{t}} \end{bmatrix} \\ &+\begin{bmatrix} 0 & 0 & 0 & 0\\ -\mathbf{R}_{t}^{b_{k}} & 0 & 0 & 0 \\ 0 & -\mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{n}_{a} \\ \mathbf{n}_{w} \\ \mathbf{n}_{b_{a}} \\ \mathbf{n}_{b_{w}} \end{bmatrix} =\mathbf{F}_{t}\delta\mathbf{z}_{t}^{b_{k}} + \mathbf{G}_{t}\mathbf{n}_{t} \end{aligned} \tag{7} ⎣⎢⎢⎢⎢⎢⎡δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙wt⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡00000I00000−Rtbk⌊a^t−bat⌋×−⌊ω^t−bwt⌋×000−Rtbk00000−I00⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡δαtbkδβtbkδθtbkδbatδbwt⎦⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎡0−Rtbk00000−I00000I00000I⎦⎥⎥⎥⎥⎤⎣⎢⎢⎡nanwnbanbw⎦⎥⎥⎤=Ftδztbk+Gtnt(7)
下面是推导过程，在进行推导前，必须明确: PVQ增量误差的导数=实际PVQ增量的导数-名义PVQ增量的导数
(1) δ α ˙ t b k \delta \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} δα˙tbk的推导：
δ α ˙ t b k = α ˙ t b k − α ^ ˙ t b k = β t b k − β ^ t b k = δ β t b k \delta \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} = \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} - \dot{\hat{\boldsymbol{\alpha}}}_{t}^{b_{k}} = \boldsymbol{\beta}_{t}^{b_{k}} - \hat{\boldsymbol{\beta}}_{t}^{b_{k}}= \delta {\boldsymbol{\beta}}_{t}^{b_{k}} δα˙tbk=α˙tbk−α^˙tbk=βtbk−β^tbk=δβtbk
(2) δ b ˙ a t \delta \dot{\mathbf{b}}_{a_{t}} δb˙at的推导：
δ b ˙ a t = b ˙ a t − b ^ ˙ a t = n b a − 0 = n b a \delta \dot{\mathbf{b}}_{a_{t}} = \dot{\mathbf{b}}_{a_{t}} - \dot{\hat{\mathbf{b}}}_{a_{t}} = \mathbf{n}_{b_{a}} - \mathbf{0} = \mathbf{n}_{b_{a}} δb˙at=b˙at−b^˙at=nba−0=nba
(3) δ b ˙ w t \delta \dot{\mathbf{b}}_{w_{t}} δb˙wt的推导：
δ b ˙ w t = b ˙ w t − b ^ ˙ w t = n b w − 0 = n b w \delta \dot{\mathbf{b}}_{w_{t}} = \dot{\mathbf{b}}_{w_{t}} - \dot{\hat{\mathbf{b}}}_{w_{t}} = \mathbf{n}_{b_{w}} - \mathbf{0} = \mathbf{n}_{b_{w}} δb˙wt=b˙wt−b^˙wt=nbw−0=nbw
(4) δ β t b k \delta {\boldsymbol{\beta}}_{t}^{b_{k}} δβtbk的推导：
δ β t b k = β ˙ t b k − β ^ ˙ t b k = R t b k ( a t − b a t ) − R ^ t b k ( a ^ t − b ^ a t ) = R ^ t b k e x p ( ⌊ δ θ ⌋ × ) ( a ^ t − n a − b ^ a t − δ b a ) − R ^ t b k ( a ^ t − b ^ a t ) = R ^ t b k ( I + ⌊ δ θ ⌋ × ) ( a ^ t − n a − b ^ a t − δ b a ) − R ^ t b k ( a ^ t − b ^ a t ) = R ^ t b k [ − n a − δ b a + ⌊ δ θ ⌋ × ( a ^ t − b ^ a t ) + ⌊ δ θ ⌋ × ( − n a − δ b a ) ] ≈ R ^ t b k [ − n a − δ b a + ⌊ δ θ ⌋ × ( a ^ t − b ^ a t ) ] = R ^ t b k [ − n a − δ b a + ⌊ ( a ^ t − b ^ a t ) ⌋ × δ θ ] = − R ^ t b k ⌊ ( a ^ t − b ^ a t ) ⌋ × δ θ − R ^ t b k δ b a t − R ^ t b k n a (8) \begin{aligned} \delta {\boldsymbol{\beta}}_{t}^{b_{k}} &= \dot{\boldsymbol{\beta}}_{t}^{b_{k}} - \dot{\hat{\boldsymbol{\beta}}}_{t}^{b_{k}} \\ &= \mathbf{R}_{t}^{b_{k}}(\mathbf{a}_{t} - \mathbf{b}_{a_{t}}) - \hat{\mathbf{R}}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) \\ &= \hat{\mathbf{R}}_{t}^{b_{k}}exp(\left\lfloor \delta \boldsymbol{\theta} \right\rfloor_{\times})(\hat{\mathbf{a}}_{t} - \mathbf{n}_{a} - \hat{\mathbf{b}}_{a_{t}}-\delta \mathbf{b}_{a}) - \hat{\mathbf{R}}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) \\ &= \hat{\mathbf{R}}_{t}^{b_{k}}(\mathbf{I}+\left\lfloor \delta \boldsymbol{\theta} \right\rfloor_{\times})(\hat{\mathbf{a}}_{t} - \mathbf{n}_{a} - \hat{\mathbf{b}}_{a_{t}}-\delta \mathbf{b}_{a}) - \hat{\mathbf{R}}_{t}^{b_{k}}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}})\\ &=\hat{\mathbf{R}}_{t}^{b_{k}}[ - \mathbf{n}_{a} -\delta \mathbf{b}_{a} + \left\lfloor \delta \boldsymbol{\theta} \right\rfloor_{\times}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) + \left\lfloor \delta \boldsymbol{\theta} \right\rfloor_{\times}( - \mathbf{n}_{a} -\delta \mathbf{b}_{a})] \\ &\approx \hat{\mathbf{R}}_{t}^{b_{k}}[ - \mathbf{n}_{a} -\delta \mathbf{b}_{a} + \left\lfloor \delta \boldsymbol{\theta} \right\rfloor_{\times}(\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) ] \\ &= \hat{\mathbf{R}}_{t}^{b_{k}}[ - \mathbf{n}_{a} -\delta \mathbf{b}_{a} + \left\lfloor (\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) \right\rfloor_{\times}\delta \boldsymbol{\theta} ] \\ &= -\hat{\mathbf{R}}_{t}^{b_{k}}\left\lfloor (\hat{\mathbf{a}}_{t} - \hat{\mathbf{b}}_{a_{t}}) \right\rfloor_{\times}\delta \boldsymbol{\theta} - \hat{\mathbf{R}}_{t}^{b_{k}}\delta \mathbf{b}_{a_{t}} - \hat{\mathbf{R}}_{t}^{b_{k}}\mathbf{n}_{a} \end{aligned} \tag{8} δβtbk=β˙tbk−β^˙tbk=Rtbk(at−bat)−R^tbk(a^t−b^at)=R^tbkexp(⌊δθ⌋×)(a^t−na−b^at−δba)−R^tbk(a^t−b^at)=R^tbk(I+⌊δθ⌋×)(a^t−na−b^at−δba)−R^tbk(a^t−b^at)=R^tbk[−na−δba+⌊δθ⌋×(a^t−b^at)+⌊δθ⌋×(−na−δba)]≈R^tbk[−na−δba+⌊δθ⌋×(a^t−b^at)]=R^tbk[−na−δba+⌊(a^t−b^at)⌋×δθ]=−R^tbk⌊(a^t−b^at)⌋×δθ−R^tbkδbat−R^tbkna(8)
(5) δ θ ˙ t b k \delta \dot{\boldsymbol{\theta}}_{t}^{b_{k}} δθ˙tbk的推导：
γ ˙ t b k = 1 2 γ t b k ⊗ [ 0 ω t − b w t ] = 1 2 γ ^ t b k ⊗ δ q ⊗ [ 0 ω ^ t − n w − b ^ w t − δ b w t ] (9) \dot{\boldsymbol{\gamma}}_{t}^{b_{k}} = \frac{1}{2}\boldsymbol{\gamma}_{t}^{b_{k}}\otimes\begin{bmatrix} 0 \\ \boldsymbol{\omega}_{t} - \mathbf{b}_{w_{t}} \end{bmatrix}=\frac{1}{2}\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta\mathbf{q} \otimes \begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} -\mathbf{n}_{w} - \hat{\mathbf{b}}_{w_{t}} - \delta\mathbf{b}_{w_{t}} \end{bmatrix} \tag{9} γ˙tbk=21γtbk⊗[0ωt−bwt]=21γ^tbk⊗δq⊗[0ω^t−nw−b^wt−δbwt](9)
又因为:
γ ˙ t b k = γ ^ t b k ⊗ δ q ˙ = γ ^ ˙ t b k ⊗ δ q + γ ^ t b k ⊗ δ q ˙ = 1 2 γ ^ t b k ⊗ [ 0 ω ^ t − b ^ w t ] ⊗ δ q + γ ^ t b k ⊗ δ q ˙ (10) \dot{\boldsymbol{\gamma}}_{t}^{b_{k}} = \dot{\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta \mathbf{q}} = \dot{\hat{\boldsymbol{\gamma}}}_{t}^{b_{k}}\otimes\delta \mathbf{q} + \hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta\dot{\mathbf{q}} = \frac{1}{2}\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} - \hat{\mathbf{b}}_{w_{t}} \end{bmatrix}\otimes\delta \mathbf{q} + \hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta\dot{\mathbf{q}} \tag{10} γ˙tbk=γ^tbk⊗δq˙=γ^˙tbk⊗δq+γ^tbk⊗δq˙=21γ^tbk⊗[0ω^t−b^wt]⊗δq+γ^tbk⊗δq˙(10)
联立公式(9)(10)可得:
1 2 γ ^ t b k ⊗ δ q ⊗ [ 0 ω ^ t − n w − b ^ w t − δ b w t ] = 1 2 γ ^ t b k ⊗ [ 0 ω ^ t − b ^ w t ] ⊗ δ q + γ ^ t b k ⊗ δ q ˙ ⇒ 1 2 δ q ⊗ [ 0 ω ^ t − n w − b ^ w t − δ b w t ] = 1 2 [ 0 ω ^ t − b ^ w t ] ⊗ δ q + δ q ˙ ⇒ 1 2 Q − δ q = 1 2 Q + δ q + δ q ˙ ⇒ δ q ˙ = 1 2 ( Q − − Q + ) δ q ⇒ [ 0 1 2 δ θ ˙ ] = 1 2 ( Q − − Q + ) [ 1 1 2 δ θ ] ⇒ [ 0 δ θ ˙ ] = [ 0 ( n w + δ b w t ) T − n w − δ b w t ⌊ − ( 2 ω ^ t − 2 b w t − n w − δ b w t ) ⌋ × ] [ 1 1 2 δ θ ] ⇒ δ θ ˙ = ⌊ − ( 2 ω ^ t − 2 b w t − n w − δ b w t ) ⌋ × 1 2 δ θ − n w − δ b w t ⇒ δ θ ˙ ≈ ⌊ − ( ω ^ t − b w t ) ⌋ × δ θ − n w − δ b w t (11) \begin{aligned} &\frac{1}{2}\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta\mathbf{q} \otimes \begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} -\mathbf{n}_{w} - \hat{\mathbf{b}}_{w_{t}} - \delta\mathbf{b}_{w_{t}} \end{bmatrix} = \frac{1}{2}\hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} - \hat{\mathbf{b}}_{w_{t}} \end{bmatrix}\otimes\delta \mathbf{q} + \hat{\boldsymbol{\gamma}}_{t}^{b_{k}}\otimes\delta\dot{\mathbf{q}} \\ & \Rightarrow \frac{1}{2}\delta\mathbf{q} \otimes \begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} -\mathbf{n}_{w} - \hat{\mathbf{b}}_{w_{t}} - \delta\mathbf{b}_{w_{t}} \end{bmatrix} = \frac{1}{2}\begin{bmatrix} 0 \\ \hat{\boldsymbol{\omega}}_{t} - \hat{\mathbf{b}}_{w_{t}} \end{bmatrix}\otimes\delta \mathbf{q} + \delta\dot{\mathbf{q}} \\ & \Rightarrow \frac{1}{2}\mathbf{Q}^{-}\delta{\mathbf{q}} = \frac{1}{2} \mathbf{Q}^{+}\delta \mathbf{q} + \delta\dot{\mathbf{q}} \\ & \Rightarrow \delta\dot{\mathbf{q}} = \frac{1}{2}(\mathbf{Q^{-}-Q^{+}})\delta \mathbf{q} \\ & \Rightarrow \begin{bmatrix} 0 \\ \frac{1}{2}\delta\dot{\boldsymbol{\theta}} \end{bmatrix} = \frac{1}{2}(\mathbf{Q^{-}-Q^{+}})\begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta}\end{bmatrix} \\ & \Rightarrow \begin{bmatrix} 0 \\ \delta\dot{\boldsymbol{\theta}} \end{bmatrix} = \begin{bmatrix} \mathbf{0} & (\mathbf{n}_{w}+\delta\mathbf{b}_{w_{t}})^{T} \\ -\mathbf{n}_{w}-\delta\mathbf{b}_{w_{t}} & \left\lfloor -(2\hat\boldsymbol{\omega}_{t} - 2\mathbf{b}_{w_{t}}-\mathbf{n}_{w}-\delta\mathbf{b}_{w_{t}}) \right\rfloor_{\times} \end{bmatrix} \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta}\end{bmatrix} \\ & \Rightarrow \delta\dot{\boldsymbol{\theta}} = \left\lfloor -(2\hat\boldsymbol{\omega}_{t} - 2\mathbf{b}_{w_{t}}-\mathbf{n}_{w}-\delta\mathbf{b}_{w_{t}}) \right\rfloor_{\times}\frac{1}{2}\delta{\boldsymbol{\theta}} - \mathbf{n}_{w}-\delta\mathbf{b}_{w_{t}} \\ & \Rightarrow \delta\dot{\boldsymbol{\theta}} \approx \left\lfloor -(\hat\boldsymbol{\omega}_{t} - \mathbf{b}_{w_{t}}) \right\rfloor_{\times}\delta{\boldsymbol{\theta}} - \mathbf{n}_{w}-\delta\mathbf{b}_{w_{t}} \end{aligned} \tag{11} 21γ^tbk⊗δq⊗[0ω^t−nw−b^wt−δbwt]=21γ^tbk⊗[0ω^t−b^wt]⊗δq+γ^tbk⊗δq˙⇒21δq⊗[0ω^t−nw−b^wt−δbwt]=21[0ω^t−b^wt]⊗δq+δq˙⇒21Q−δq=21Q+δq+δq˙⇒δq˙=21(Q−−Q+)δq⇒[021δθ˙]=21(Q−−Q+)[121δθ]⇒[0δθ˙]=[0−nw−δbwt(nw+δbwt)T⌊−(2ω^t−2bwt−nw−δbwt)⌋×][121δθ]⇒δθ˙=⌊−(2ω^t−2bwt−nw−δbwt)⌋×21δθ−nw−δbwt⇒δθ˙≈⌊−(ω^t−bwt)⌋×δθ−nw−δbwt(11)
上式中的 ( Q + , Q − ) (\mathbf{Q^{+},Q^{-}}) (Q+,Q−)为四元素的左乘右乘矩阵，其计算方法可参考Quaternion kinematics for ESKF－预测中"一些四元素的性质"章节．
PVQ增量误差的离散形式(此处参考崔华坤大佬的笔记)

⇒ δ z k + 1 15 × 1 = F 15 × 15 δ z k 15 × 1 + V 15 × 18 Q 18 × 1 (12) \Rightarrow {\delta \mathbf{z}_{k+1}}^{15\times1} = {\mathbf{F}}^{15\times15}{\delta{z}_{k}}^{15\times1} + {\mathbf{V}}^{15\times18}{\mathbf{Q}}^{18\times1} \tag{12} ⇒δzk+115×1=F15×15δzk15×1+V15×18Q18×1(12)
式中：

推导:
PVQ增量协方差传递方程
由公式(12)可知协方差传递方程为:
P k + 1 15 × 15 = F P k F T + V Q V T (13) \mathbf{P}_{k+1}^{15\times15} = \mathbf{F}\mathbf{P}_{k}\mathbf{F}^{T} + \mathbf{VQV}^{T} \tag{13} Pk+115×15=FPkFT+VQVT(13)

源码解析

    void midPointIntegration(double _dt,const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian){// PVQ增量中值积分，对应公式(3)//ROS_INFO("midpoint integration");Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // 对应公式(4)中的平均角速度result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); // 对应公式(3)中的姿态增量Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); // 对应公式(4)中的平均加速度result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt; // 对应公式(3)中的位置增量result_delta_v = delta_v + un_acc * _dt; // 对应公式(3)中的速度增量result_linearized_ba = linearized_ba; // 预积分过程中偏置不变result_linearized_bg = linearized_bg;if (update_jacobian){Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;Vector3d a_0_x = _acc_0 - linearized_ba;Vector3d a_1_x = _acc_1 - linearized_ba;Matrix3d R_w_x, R_a_0_x, R_a_1_x;R_w_x << 0, -w_x(2), w_x(1),w_x(2), 0, -w_x(0),-w_x(1), w_x(0), 0;R_a_0_x << 0, -a_0_x(2), a_0_x(1),a_0_x(2), 0, -a_0_x(0),-a_0_x(1), a_0_x(0), 0;R_a_1_x << 0, -a_1_x(2), a_1_x(1),a_1_x(2), 0, -a_1_x(0),-a_1_x(1), a_1_x(0), 0;// F = df/dx// x = {p, theta, v, b, ba, bg}// 对应公式(12)中的F矩阵MatrixXd F = MatrixXd::Zero(15, 15);F.block<3, 3>(0, 0) = Matrix3d::Identity();F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * _dt;F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * _dt;F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;F.block<3, 3>(6, 6) = Matrix3d::Identity();F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;F.block<3, 3>(9, 9) = Matrix3d::Identity();F.block<3, 3>(12, 12) = Matrix3d::Identity();//cout<<"A"<<endl<<A<<endl;// 对应公式(12)的V矩阵MatrixXd V = MatrixXd::Zero(15, 18);V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * _dt;V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * _dt;V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * _dt;V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * _dt;//step_jacobian = F;//step_V = V;// 更新jacobijacobian = F * jacobian;                                                 // 误差传递(更新预积分后状态量的协方差矩阵)covariance = F * covariance * F.transpose() + V * noise * V.transpose(); }}

代码注释

https://github.com/chennuo0125-HIT/vins-note

VINS-Mono-IMU预积分原理及源码解析相关推荐

VINS学习（二）IMU预积分原理与实现
VINS学习(二)IMU预积分原理与实现一.连续时间下的IMU积分二.连续时间下的IMU预积分三.离散时间下的IMU预积分 1. 欧拉法 2. 中值法四.连续时间下的IMU状态误差传递五.离 ...
谷歌BERT预训练源码解析（三）：训练过程
目录前言源码解析主函数自定义模型遮蔽词预测下一句预测规范化数据集前言本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...
谷歌BERT预训练源码解析（一）：训练数据生成
目录预训练源码结构简介输入输出源码解析参数主函数创建训练实例下一句预测&实例生成随机遮蔽输出结果一览预训练源码结构简介关于BERT,简单来说,它是一个基于Transfo ...
谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一) 1. LiDAR inertial odometry and mapping简介 ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(四) 3. Joint optimization 3.3 IMU preintegrat ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(二) 3. Joint optimization 3.1 Marginalization ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（三）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(三) 3. Joint optimization 3.2 IMU preintegrat ...
Redis源码解析——前言
今天开启Redis源码的阅读之旅.对于一些没有接触过开源代码分析的同学来说,可能这是一件很麻烦的事.但是我总觉得做一件事,不管有多大多难,我们首先要在战略上蔑视它,但是要在战术上重视它.除了一些高大上 ...

VINS-Mono-IMU预积分原理及源码解析

IMU预积分原理

PVQ增量预积分(可理解为EKF过程中的状态估计)

误差传递(可理解为EKF过程中的协方差估计)

源码解析

代码注释

VINS-Mono-IMU预积分原理及源码解析相关推荐

最新文章

热门文章