VINS-Mono(一)——IMU预积分及系统初始化

文章目录

- Reference
- 预积分的推导
- - ESKF
  - 积分与预积分
  - VINS中的中点法（mid-point）
  - - 旋转的error-state
    - 位移的error-state
    - 速度的error-state
    - 零偏Bias的error-state
    - 整个状态转移方程的矩阵形式
- visual-inertial alignment
- - - 对陀螺仪初始零偏的估计
    - Visual Inertial Alignment
    - Refine Gravity

Reference

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.
Quaternion Kinematics for Error-State KF.

预积分的推导

预积分是VINS-Mono中十分重要的一个步骤，主要涉及的是IMU的运动方程，下面对这部分进行一个简单的推导，主要使用的是ESKF中的方法，采用简单的欧拉法在连续时域下进行推导。

ESKF

这部分详细可以参照参考【2】，这里做一个记录，ESKF的主要精髓在于分析的是error-state，并且获得的是error-state的状态更新方程，但是对于优化问题，优化的变量其实并不是error-state，而是state，那为什么会有这个理论的诞生，这里先埋个坑（坑1），后面会解释。

ESKF有两个变量，一个是normal-state X X X，一个是truth-state X t X_t Xt，两者的关系如下：

X t = [ p t v t q t a b t ω b t ] = [ p + δ p v + δ v q ⊗ δ q a b + δ a b ω b + δ ω b ] = X ⊞ δ X (1) \mathbf{X_t}=\left[\begin{array}{c} \mathbf{p_t} \\ \mathbf{v_t} \\ \mathbf{q_t} \\ \mathbf{a}_{bt} \\ \boldsymbol{\omega}_{bt} \end{array}\right]= \left[\begin{array}{c} \mathbf{p}+\delta{p} \\ \mathbf{v}+\delta{v} \\ \mathbf{q}\otimes\delta{q} \\ \mathbf{a}_{b}+\delta{a_b} \\ \boldsymbol{\omega}_{b}+\delta{\omega_b} \end{array}\right]=\mathbf{X}\boxplus\mathbf{\delta{X}} \tag{1} Xt=⎣⎢⎢⎢⎢⎡ptvtqtabtωbt⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡p+δpv+δvq⊗δqab+δabωb+δωb⎦⎥⎥⎥⎥⎤=X⊞δX(1)
进一步，通过对两边求微分得到：

δ p ˙ = δ v δ v ˙ = − R [ a m − a b ] × δ θ − R δ a b − R a n δ θ ˙ = − [ ω m − ω b ] × δ θ − δ ω b − ω n δ a b ˙ = a w δ ω ˙ b = ω w (2) \begin{aligned} \dot{\delta \mathbf{p}} &=\delta \mathbf{v} \\ \dot{\delta \mathbf{v}} &=-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}-\mathbf{R} \mathbf{a}_{n} \\ \dot{\delta \boldsymbol{\theta}} &=-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \\ \dot{\delta \mathbf{a}_{b}} &=\mathbf{a}_{w} \\ \delta \dot{\boldsymbol{\omega}}_{b} &=\boldsymbol{\omega}_{w} \end{aligned} \tag{2} δp˙δv˙δθ˙δab˙δω˙b=δv=−R[am−ab]×δθ−Rδab−Ran=−[ωm−ωb]×δθ−δωb−ωn=aw=ωw(2)
对公式2进行离散化

δ p ← δ p + δ v Δ t δ v ← δ v + ( − R [ a m − a b ] × δ θ − R δ a b ) Δ t + v i δ θ ← I − [ ω m − ω b ] × Δ t δ θ − δ ω b Δ t + θ i δ a b ← δ a b + a i δ ω b ← δ ω b + ω i (3) \begin{aligned} \delta \mathbf{p} & \leftarrow \delta \mathbf{p}+\delta \mathbf{v} \Delta t \\ \delta \mathbf{v} & \leftarrow \delta \mathbf{v}+\left(-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}\right) \Delta t+\mathbf{v}_{\mathbf{i}} \\ \delta \boldsymbol{\theta} & \leftarrow I-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \Delta t \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b} \Delta t+\boldsymbol{\theta}_{\mathbf{i}} \\ \delta \mathbf{a}_{b} & \leftarrow \delta \mathbf{a}_{b}+\mathbf{a}_{\mathbf{i}} \\ \delta \boldsymbol{\omega}_{b} & \leftarrow \delta \boldsymbol{\omega}_{b}+\boldsymbol{\omega}_{\mathbf{i}} \end{aligned} \tag{3} δpδvδθδabδωb←δp+δvΔt←δv+(−R[am−ab]×δθ−Rδab)Δt+vi←I−[ωm−ωb]×Δtδθ−δωbΔt+θi←δab+ai←δωb+ωi(3)

其中：
- R R R表示从机体系到世界坐标系的旋转；
- a m a_m am表示IMU的测量减去重力加速度的值；
将公式3写作矩阵形式，得到kalman滤波的预测阶段：

δ ^ x ← F x ( x , u m ) ⋅ δ ^ x P ← F x P F x ⊤ + F i Q i F i ⊤ (4) \begin{array}{l} \hat{\delta} \boldsymbol{\mathbf { x }} \leftarrow \mathbf{F}_{\mathbf{x}}\left(\mathbf{x}, \mathbf{u}_{m}\right) \cdot \hat{\boldsymbol{\delta}} \mathbf{x} \\ \mathbf{P} \leftarrow \mathbf{F}_{\mathbf{x}} \mathbf{P} \mathbf{F}_{\mathbf{x}}^{\top}+\mathbf{F}_{\mathbf{i}} \mathbf{Q}_{\mathbf{i}} \mathbf{F}_{\mathbf{i}}^{\top} \end{array} \tag{4} δ^x←Fx(x,um)⋅δ^xP←FxPFx⊤+FiQiFi⊤(4)

其中：
- F x = ∂ f ∂ δ x ∣ x , u m = [ I I Δ t 0 0 0 0 0 I − R [ a m − a b ] × Δ t − R Δ t 0 I Δ t 0 0 I − [ ω m − ω b ] × Δ t 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{F}_{\mathbf{x}}=\left.\frac{\partial f}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{ccccc} \mathbf{I} & \mathbf{I} \Delta t & 0 & 0 & 0 & 0 \\ 0 & \mathbf{I} & -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} \Delta t & -\mathbf{R} \Delta t & 0 & \mathbf{I} \Delta t \\ 0 & 0 & I-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{\times} \Delta t & 0 & -\mathbf{I} \Delta t & 0 \\ 0 & 0 & 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & 0 & 0 & \mathbf{I} \end{array}\right] Fx=∂δx∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡I00000IΔtI00000−R[am−ab]×ΔtI−[ωm−ωb]×Δt0000−RΔt0I0000−IΔt0I00IΔt000I⎦⎥⎥⎥⎥⎥⎥⎤
- F i = ∂ f ∂ i ∣ x , u m = [ 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 I 0 0 0 0 ] \mathbf{F}_{\mathbf{i}}=\left.\frac{\partial f}{\partial \mathbf{i}}\right|_{\mathbf{x}, \mathbf{u}_{m}}=\left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ \mathbf{I} & 0 & 0 & 0 \\ 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & \mathbf{I} & 0 \\ 0 & 0 & 0 & \mathbf{I} \\ 0 & 0 & 0 & 0 \end{array}\right] Fi=∂i∂f∣∣∣∣x,um=⎣⎢⎢⎢⎢⎢⎢⎡0I000000I000000I000000I0⎦⎥⎥⎥⎥⎥⎥⎤
- Q i = [ V i 0 0 0 0 Θ i 0 0 0 0 A i 0 0 0 0 Ω i ] \mathbf{Q}_{\mathbf{i}}=\left[\begin{array}{cccc} \mathbf{V}_{\mathbf{i}} & 0 & 0 & 0 \\ 0 & \mathbf{\Theta}_{\mathbf{i}} & 0 & 0 \\ 0 & 0 & \mathbf{A}_{\mathbf{i}} & 0 \\ 0 & 0 & 0 & \Omega_{\mathbf{i}} \end{array}\right] Qi=⎣⎢⎢⎡Vi0000Θi0000Ai0000Ωi⎦⎥⎥⎤

积分与预积分

积分比较简单，拿到IMU的数据进行常规的积分操作，就可以得到两节点之间的相对位姿和速度。这里祭出IMU的运动模型：

{ p ˙ = v v ˙ = R i k i 0 ( a m − b a − n a ) q ˙ = q ⊗ q { w m − b w − n w } b a ˙ = w a b w ˙ = w w (5) \begin{cases} \dot{p}=v \\ \dot{v}=R_{i_k}^{i_0}(a_m-b_a-n_a) \\ \dot{q}=q\otimes q\{w_m-b_w-n_w\} \\ \dot{b_a}=w_a \\ \dot{b_w}=w_w \end{cases} \tag{5} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧p˙=vv˙=Riki0(am−ba−na)q˙=q⊗q{wm−bw−nw}ba˙=wabw˙=ww(5)

可以看到，在进行积分的时候，公式中不仅使用了各个时刻的IMU测量值，而且用到零偏值，因此当零偏值变化的时候，整个微分数值就产生了变化，于是最后的相对状态值就会产生变化，此时一种方法是将修改后的零偏值带入到积分过程中得到新的相对位姿和速度。

重新带入的方法是可行的，但是这样会比较浪费时间和算力。于是预积分的方法就提出来了。

承接上面的ESKF，把正常的积分过程看做是normal-state，这部分积分仅与这段时间的IMU测量值以及零偏bias有关，与其他量都无关，当零偏bias被优化之后，此时并不用修改normal-state的量，而是认为变化的这部分在error-state中，下面有个简略的示意图：

VINS-Mono(一)——IMU预积分及系统初始化

图中X表示使用IMU数据进行积分的过程，IMU认为在这段时间内，坐标系的变化为从 I k I_k Ik到 I k + 1 I_{k+1} Ik+1，而当状态变量进行了优化，零偏变化 δ b \delta b δb时，积分过程需要加上 Δ X \Delta X ΔX，即假如零偏变化了 δ b \delta b δb，那么IMU认为坐标系的变化应该到 I k + 1 ′ I^{'}_{k+1} Ik+1′，所以剩下就是求解上图中的黄色部分的变化了，这里采用一阶近似来表示这个变化，推导如下：

{ p n t = p n + ∂ p ∂ b δ b = p n + p + δ p − p δ b δ b = p n + δ p δ b δ b v n t = v n + ∂ v ∂ b δ b = v n + v + δ v − v δ b δ b = v n + δ v δ b δ b q n t = q n ⊗ [ 1 1 2 ∂ θ ∂ b δ b ] = q n ⊗ [ 1 1 2 ∂ θ ∂ b δ b ] = q n ⊗ [ 1 1 2 θ + δ θ − θ δ b δ b ] = q n ⊗ [ 1 1 2 δ θ δ b δ b ] (6) \begin{cases} p_{nt}=p_{n}+\frac{\partial{p}}{\partial{b}}\delta{b}=p_{n}+\frac{p+\delta{p}-p}{\delta{b}}\delta{b}=p_{n}+\frac{\delta{p}}{\delta{b}}\delta{b} \\ v_{nt}=v_{n}+\frac{\partial{v}}{\partial{b}}\delta{b}=v_{n}+\frac{v+\delta{v}-v}{\delta{b}}\delta{b}=v_{n}+\frac{\delta{v}}{\delta{b}}\delta{b} \\ q_{nt}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\partial{\theta}}{\partial{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\partial{\theta}}{\partial{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\theta+\delta{\theta}-\theta}{\delta{b}}\delta{b} \end{bmatrix}=q_{n}\otimes\begin{bmatrix}1\\ \frac{1}{2}\frac{\delta{\theta}}{\delta{b}}\delta{b} \end{bmatrix} \end{cases} \tag{6} ⎩⎪⎪⎪⎨⎪⎪⎪⎧pnt=pn+∂b∂pδb=pn+δbp+δp−pδb=pn+δbδpδbvnt=vn+∂b∂vδb=vn+δbv+δv−vδb=vn+δbδvδbqnt=qn⊗[121∂b∂θδb]=qn⊗[121∂b∂θδb]=qn⊗[121δbθ+δθ−θδb]=qn⊗[121δbδθδb](6)

其中需要说明的几个点为：

偏导这里采用了求导的原始定义；
相对姿态使用四元数进行表示，这里使用旋转向量来表示误差量，这样的好处就是能在 R 3 \R^3 R3空间上直接进行向量的加减操作，十分方便；

剩下的就是如何求解上述推导中的 δ p δ b , δ v δ b , δ q δ b \frac{\delta{p}}{\delta{b}},\frac{\delta{v}}{\delta{b}},\frac{\delta{q}}{\delta{b}} δbδp,δbδv,δbδq了，特别注意的这里的三个导数和公式4中的变量并不一样，因为公式（4）是两个IMU周期之间的递推公式，而公式（6）是两个相机周期（一般都是N倍于IMU周期）之间的递推公式，那这个数值是多少呢？这里其实是一个连乘的操作，如下：

δ p n δ b = F n − 1 δ p n − 1 δ b = F n − 1 F n − 2 δ p n − 2 δ b = ∏ i = 0 n − 1 F i δ p 0 δ b = ∏ i = 0 n − 1 F i where δ p 0 δ b = I \frac{\delta{p_n}}{\delta{b}}=\frac{F_{n-1}\delta{p_{n-1}}}{\delta{b}}=\frac{F_{n-1}F_{n-2}\delta{p_{n-2}}}{\delta{b}}=\frac{\prod_{i=0}^{n-1}F_{i}\delta{p_0}}{\delta{b}}=\prod_{i=0}^{n-1}F_{i} \text{ where } \frac{\delta{p_{0}}}{\delta{b}}=I δbδpn=δbFn−1δpn−1=δbFn−1Fn−2δpn−2=δb∏i=0n−1Fiδp0=i=0∏n−1Fi where δbδp0=I

所以为什么要用ESKF也就很明显了，同时使用ESKF进行error-state状态的更新还有另一个好处就是能够得到这次观测的协方差，如何得到呢？其实也很简单，使用协方差的原始定义就好啦，这里以位置为例：

P p = E ( ( p t − p i n t ) T ( p t − p i n t ) ) = E ( δ p T δ p ) = P δ p (7) P_p=E((p_t-p_{int})^T(p_t-p_{int}))=E(\delta{p}^T\delta{p})=P_{\delta{p}} \tag{7} Pp=E((pt−pint)T(pt−pint))=E(δpTδp)=Pδp(7)

VINS中的中点法（mid-point）

在VINS-Mono的代码中，作者使用的是中点法得到上面的公式4的，其实和参考【2】中的中点法不太一样，这里的中点法使用的是两端点的平均，下面进行简单的推导，先给出离散时间下的迭代步骤：

{ p k + 1 = p k + v k Δ t + 1 2 R k ( a k − b a k ) + R k + 1 ( a k + 1 − b a k ) 2 Δ t 2 v k + 1 = v k + R k ( a k − b k ) + R k + 1 ( a k + 1 − b k + 1 ) 2 Δ t q k + 1 = q k ⊗ q { ( ( w k + w k + 1 ) 2 − b w k ) Δ t } b a k + 1 = b a k + n b a Δ t b w k + 1 = b w k + n b w Δ t (8) \begin{cases} p_{k+1}=p_{k}+v_{k}\Delta{t}+\frac{1}{2}\frac{R_k(a_k-b_{ak})+R_{k+1}(a_{k+1}-b_{ak})}{2}\Delta{t}^2 \\ v_{k+1}=v_k+\frac{R_k(a_k-b_k)+R_{k+1}(a_{k+1}-b_{k+1})}{2}\Delta{t}\\ q_{k+1}=q_{k}\otimes q\{(\frac{(w_k+w_{k+1})}{2}-b_{wk})\Delta{t}\}\\ b_{ak+1}=b_{ak}+n_{ba}\Delta{t} \\ b_{wk+1}=b_{wk}+n_{bw}\Delta{t} \end{cases} \tag{8} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧pk+1=pk+vkΔt+212Rk(ak−bak)+Rk+1(ak+1−bak)Δt2vk+1=vk+2Rk(ak−bk)+Rk+1(ak+1−bk+1)Δtqk+1=qk⊗q{(2(wk+wk+1)−bwk)Δt}bak+1=bak+nbaΔtbwk+1=bwk+nbwΔt(8)

整个过程中稍微有些奇怪的是VINS在表示陀螺仪的真值的时候，使用的是 w t = w m − b w − δ b w + n w w_t=w_m-b_w-\delta{b_w}+n_w wt=wm−bw−δbw+nw，不过因为噪声是高斯白噪声，加和减是一样的，所以这里按这样推导也无妨。

旋转的error-state

这里先推导旋转的误差项是因为后面的位移和速度会用到这部分结论，整体来说和上面的ESKF一样，使用truth-state=normal-state+error-state的方法进行推导，这部分沿用微分的方式：

q t ˙ = ( q ⊗ δ q ) ˙ = 1 2 q t ⊗ Ω t = q ˙ ⊗ δ q + q ⊗ δ q ˙ = 1 2 q t ⊗ Ω t = 1 2 q ⊗ Ω ⊗ δ q + q ⊗ δ q ˙ = 1 2 q ⊗ δ q ⊗ Ω t = [ Ω ] L δ q + 2 δ q ˙ = [ Ω t ] R δ q = 2 δ q ˙ = ( [ Ω t ] R − [ Ω ] L ) δ q (9) \begin{aligned} \dot{q_{t}}&=\dot{(q\otimes \delta{q})}=\frac{1}{2}q_t\otimes \Omega_t \\ &=\dot{q}\otimes \delta{q}+q \otimes \dot{\delta{q}}=\frac{1}{2}q_t\otimes \Omega_t \\ &=\frac{1}{2}q \otimes \Omega \otimes \delta{q}+q\otimes \dot{\delta{q}}=\frac{1}{2}q\otimes\delta{q}\otimes \Omega_t \\ &=[\Omega]_L\delta{q}+2\dot{\delta{q}} = [\Omega_t]_R \delta{q} \\ &=2\dot{\delta{q}} = ([\Omega_t]_R-[\Omega]_L)\delta{q} \end{aligned} \tag{9} qt˙=(q⊗δq)˙=21qt⊗Ωt=q˙⊗δq+q⊗δq˙=21qt⊗Ωt=21q⊗Ω⊗δq+q⊗δq˙=21q⊗δq⊗Ωt=[Ω]Lδq+2δq˙=[Ωt]Rδq=2δq˙=([Ωt]R−[Ω]L)δq(9)

下面对其中的一些符号进行说明：

δ q = [ 1 1 2 δ θ ] \delta{q}=\begin{bmatrix} 1 \\ \frac{1}{2}\delta{\theta} \end{bmatrix} δq=[121δθ]
[ Ω ] R = [ 0 − Ω T Ω − [ Ω ] × ] [ Ω ] L = [ 0 − Ω T Ω [ Ω ] × ] \begin{aligned} [\Omega]_R&= \begin{bmatrix}0 & -\Omega^T \\ \Omega & -[\Omega]_{\times} \end{bmatrix} \\ [\Omega]_L&= \begin{bmatrix}0 & -\Omega^T \\ \Omega & [\Omega]_{\times} \end{bmatrix} \end{aligned} [Ω]R[Ω]L=[0Ω−ΩT−[Ω]×]=[0Ω−ΩT[Ω]×]
Ω = w m k + w m k + 1 2 − b w k Ω t = w m k + n w k + w m k + 1 + n w k + 1 2 − b w k − δ b w k = Ω − δ b w k + 1 2 n w k + 1 2 n w k + 1 \begin{aligned} \Omega &=\frac{w^k_m+w^{k+1}_m}{2}-b^k_w \\ \Omega_t&=\frac{w^k_m+n^k_w+w^{k+1}_m+n^{k+1}_{w}}{2}-b^k_w-\delta{b^k_w}=\Omega-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \\ \end{aligned} ΩΩt=2wmk+wmk+1−bwk=2wmk+nwk+wmk+1+nwk+1−bwk−δbwk=Ω−δbwk+21nwk+21nwk+1

至此，把上述的展开代入到公式9最后一行中中：

[ 0 δ θ ˙ ] = [ 0 − ( Ω t − Ω ) T ( Ω t − Ω ) − ( [ Ω t ] × + [ Ω ] × ) ] [ 1 1 2 δ θ ] = [ 0 − ( − δ b w k + 1 2 n w k + 1 2 n w k + 1 ) T − δ b w k + 1 2 n w k + 1 2 n w k + 1 − ( 2 [ Ω ] − [ δ b w k × + 1 2 n w k + 1 2 n w k + 1 ] × ) ] [ 1 1 2 δ θ ] = [ . . . − [ Ω ] × δ θ − δ b w k + 1 2 n w k + 1 2 n w k + 1 ] δ θ ˙ → − [ Ω ] × δ θ − δ b w k + 1 2 n w k + 1 2 n w k + 1 (10) \begin{aligned} \begin{bmatrix}0 \\ \dot{\delta{\theta}} \end{bmatrix}&=\begin{bmatrix}0 & -(\Omega_t-\Omega)^T \\ (\Omega_t-\Omega) & -([\Omega_t]_{\times}+[\Omega]_{\times}) \end{bmatrix} \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix} \\ &=\begin{bmatrix}0 & -(-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w})^T \\ -\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} & -(2[\Omega]-[\delta{b^k_w}_{\times}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w}]_{\times})\end{bmatrix}\begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix} \\ &=\begin{bmatrix}... \\ -[\Omega]_{\times}\delta{\theta}-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \end{bmatrix} \\ \dot{\delta{\theta}}&\rightarrow -[\Omega]_{\times}\delta{\theta}-\delta{b^k_w}+\frac{1}{2}n^{k}_{w}+\frac{1}{2}n^{k+1}_{w} \end{aligned} \tag{10} [0δθ˙]δθ˙=[0(Ωt−Ω)−(Ωt−Ω)T−([Ωt]×+[Ω]×)][121δθ]=[0−δbwk+21nwk+21nwk+1−(−δbwk+21nwk+21nwk+1)T−(2[Ω]−[δbwk×+21nwk+21nwk+1]×)][121δθ]=[...−[Ω]×δθ−δbwk+21nwk+21nwk+1]→−[Ω]×δθ−δbwk+21nwk+21nwk+1(10)

所以离散化之后：

δ θ k + 1 = δ θ k − [ Ω ] × Δ t δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t (11) \delta{\theta}_{k+1}=\delta{\theta}_k-[\Omega]_{\times}\Delta{t} \delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t} \tag{11} δθk+1=δθk−[Ω]×Δtδθk−δbwkΔt+21nwkΔt+21nwk+1Δt(11)

位移的error-state

取公式8中的位移迭代公式，并使用truth-state=normal-state+error-state的方法进行推导，有：

p k + 1 t = p k + 1 + δ p k + 1 = p k t + v k t + 1 2 R k t ( a k t − b a k t ) + R k + 1 t ( a k + 1 t − b a k t ) 2 Δ t 2 = p k + 1 + δ p k + 1 = p k + δ p k + ( v k + δ v k ) Δ t + 1 2 R k t ( a k t − b a k t ) + R k + 1 t ( a k + 1 t − b a k t ) 2 Δ t 2 \begin{aligned} p_{k+1}^{t}&=p_{k+1}+\delta{p_{k+1}}=p^{t}_{k}+v_{k}^t+\frac{1}{2}\frac{R_k^t(a_k^t-b_{ak}^t)+R_{k+1}^t(a_{k+1}^t-b_{ak}^t)}{2}\Delta{t}^2 \\ &=p_{k+1}+\delta{p_{k+1}}=p_{k}+\delta{p_k}+(v_{k}+\delta{v_k})\Delta{t}+\frac{1}{2}\frac{R_k^t(a_k^t-b_{ak}^t)+R_{k+1}^t(a_{k+1}^t-b_{ak}^t)}{2}\Delta{t}^2 \end{aligned} pk+1t=pk+1+δpk+1=pkt+vkt+212Rkt(akt−bakt)+Rk+1t(ak+1t−bakt)Δt2=pk+1+δpk+1=pk+δpk+(vk+δvk)Δt+212Rkt(akt−bakt)+Rk+1t(ak+1t−bakt)Δt2

把 p k + 1 p_{k+1} pk+1的迭代公式代入之后有：

δ p k + 1 = δ p k + δ v k Δ t + 1 4 { [ R k t ( a k t − b a k t ) − R k ( a k − b a k ) ] + [ R k + 1 t ( a k + 1 t − b a k t ) − R k + 1 ( a k + 1 − b a k ] } Δ t 2 (12) \begin{aligned} \delta{p_{k+1}}&=\delta{p_k}+\delta{v_k}\Delta{t}+\frac{1}{4}\{[R_k^t(a_k^t-b_{ak}^t)- R_k(a_k-b_{ak})]+[R_{k+1}^t(a_{k+1}^t-b_{ak}^t)-R_{k+1}(a_{k+1}-b_{ak}]\}\Delta{t}^2 \end{aligned} \tag{12} δpk+1=δpk+δvkΔt+41{[Rkt(akt−bakt)−Rk(ak−bak)]+[Rk+1t(ak+1t−bakt)−Rk+1(ak+1−bak]}Δt2(12)

把后面的大括号中的部分设为E，单独拿出来化简，有：

E = R k E x p ( δ θ k ) ( a k − n a k − b a k − δ b a k ) − R k ( a k − b a k ) + R k + 1 E x p ( δ θ k + 1 ) ( a k + 1 − n a k + 1 − b a k + 1 − δ b a k + 1 ) − R k ( a k + 1 − b a k + 1 ) = R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − n a k − b a k − δ b a k ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − n a k + 1 − b a k + 1 − δ b a k + 1 ) = R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − b a k ) + O ( ∣ ∣ δ θ k ∣ ∣ 2 ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − b a k + 1 ) + O ( ∣ ∣ δ θ k + 1 ∣ ∣ 2 ) ≈ R k ( − δ b a k − n a k ) + R k [ δ θ k ] × ( a k − b a k ) + R k + 1 ( − δ b a k + 1 − n a k + 1 ) + R k + 1 [ δ θ k + 1 ] × ( a k + 1 − b a k + 1 ) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × δ θ k + 1 where equation(11) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × ( δ θ k − [ Ω ] × Δ t δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t ) ≈ R k ( − δ b a k − n a k ) − R k [ a k − b a k ] × δ θ k + R k + 1 ( − δ b a k + 1 − n a k + 1 ) − R k + 1 [ a k + 1 − b a k + 1 ] × ( [ I − [ Ω ] × Δ t ] δ θ k − δ b w k Δ t + 1 2 n w k Δ t + 1 2 n w k + 1 Δ t ) where δ b a ˙ = 0 → δ b a k + 1 = δ b a k ≈ − ( R k [ a k − b a k ] × + R k + 1 [ a k + 1 − b a k ] × ( [ I − [ Ω ] × Δ t ] ) δ θ k ⏟ δ θ + ( − R k + R k + 1 ) δ b a k ⏟ b a k + ( R k + 1 [ a k + 1 − b a k + 1 ] × Δ t ) δ b w k ⏟ b w k + ( − R k + 1 [ a k + 1 − b a k + 1 ] × 1 2 Δ t ) n w k ⏟ n w k + ( − R k + 1 [ a k + 1 − b a k + 1 ] × 1 2 Δ t ) n w k + 1 ⏟ n w k + 1 + ( − R k ) n a k ⏟ n a k + ( − R k + 1 ) n a k + 1 ⏟ n a k + 1 (13) \begin{aligned} E&=R_kExp(\delta{\theta_k})(a_k-n^k_{a}-b_{ak}-\delta{b_{ak}})-R_k(a_k-b_{ak}) \\ &+R_{k+1}Exp(\delta{\theta_{k+1}})(a_{k+1}-n^{k+1}_{a}-b_{ak+1}-\delta{b_{ak+1}})-R_k(a_{k+1}-b_{ak+1}) \\ \\ &=R_k(-\delta{b}_{ak}-n_{a}^k)+R_k[\delta{\theta_k}]_{\times}(a_k-n^k_{a}-b_{ak}-\delta{b_{ak}})\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-n^{k+1}_{a}-b_{ak+1}-\delta{b_{ak+1}}) \\ \\ &=R_k(-\delta{b}_{ak}-n_{a}^k)+R_{k}[\delta{\theta_k}]_{\times}(a_k-b_{ak})+O(||\delta{\theta_k}||^2)+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1}) \\ &+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-b_{ak+1})+O(||\delta{\theta_{k+1}}||^2) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)+R_{k}[\delta{\theta_k}]_{\times}(a_k-b_{ak})\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})+R_{k+1}[\delta{\theta_{k+1}}]_{\times}(a_{k+1}-b_{ak+1}) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}\\ &+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\delta{\theta_{k+1}}\\ &\text{where equation(11)} \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})\\ &-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}(\delta{\theta}_k-[\Omega]_{\times}\Delta{t} \delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t}) \\ \\ &\approx R_k(-\delta{b}_{ak}-n_{a}^k)-R_{k}[a_k-b_{ak}]_{\times}\delta{\theta_k}+R_{k+1}(-\delta{b}_{ak+1}-n_{a}^{k+1})\\ &-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}([I-[\Omega]_{\times}\Delta{t}]\delta{\theta}_k-\delta{b^k_w}\Delta{t}+\frac{1}{2}n^{k}_{w}\Delta{t}+\frac{1}{2}n^{k+1}_{w}\Delta{t}) \\ &\text{where } \dot{\delta{b_a}}=0 \rightarrow \delta{b_{ak+1}}=\delta{b_{ak}} \\ \\ &\approx \underbrace{-(R_k[a_k-b_{ak}]_{\times}+R_{k+1}[a_{k+1}-b_{ak}]_{\times}([I-[\Omega]_{\times}\Delta{t}])\delta{\theta}_k}_{\delta{\theta}}\\ &+\underbrace{(-R_{k}+R_{k+1})\delta{b_{ak}}}_{b_{ak}}+\underbrace{(R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\Delta{t})\delta{b_{wk}}}_{b_{wk}}\\ &+\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\frac{1}{2}\Delta{t})n_{w}^k}_{n_{w}^k}+\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}]_{\times}\frac{1}{2}\Delta{t})n_{w}^{k+1}}_{n_{w}^{k+1}} \\ &+\underbrace{(-R_{k})n_{a}^k}_{n_{a}^k}+\underbrace{(-R_{k+1})n_{a}^{k+1}}_{n_{a}^{k+1}} \end{aligned} \tag{13} E=RkExp(δθk)(ak−nak−bak−δbak)−Rk(ak−bak)+Rk+1Exp(δθk+1)(ak+1−nak+1−bak+1−δbak+1)−Rk(ak+1−bak+1)=Rk(−δbak−nak)+Rk[δθk]×(ak−nak−bak−δbak)+Rk+1(−δbak+1−nak+1)+Rk+1[δθk+1]×(ak+1−nak+1−bak+1−δbak+1)=Rk(−δbak−nak)+Rk[δθk]×(ak−bak)+O(∣∣δθk∣∣2)+Rk+1(−δbak+1−nak+1)+Rk+1[δθk+1]×(ak+1−bak+1)+O(∣∣δθk+1∣∣2)≈Rk(−δbak−nak)+Rk[δθk]×(ak−bak)+Rk+1(−δbak+1−nak+1)+Rk+1[δθk+1]×(ak+1−bak+1)≈Rk(−δbak−nak)−Rk[ak−bak]×δθk+Rk+1(−δbak+1−nak+1)−Rk+1[ak+1−bak+1]×δθk+1where equation(11)≈Rk(−δbak−nak)−Rk[ak−bak]×δθk+Rk+1(−δbak+1−nak+1)−Rk+1[ak+1−bak+1]×(δθk−[Ω]×Δtδθk−δbwkΔt+21nwkΔt+21nwk+1Δt)≈Rk(−δbak−nak)−Rk[ak−bak]×δθk+Rk+1(−δbak+1−nak+1)−Rk+1[ak+1−bak+1]×([I−[Ω]×Δt]δθk−δbwkΔt+21nwkΔt+21nwk+1Δt)where δba˙=0→δbak+1=δbak≈δθ

−(Rk[ak−bak]×+Rk+1[ak+1−bak]×([I−[Ω]×Δt])δθk+bak

(−Rk+Rk+1)δbak+bwk

(Rk+1[ak+1−bak+1]×Δt)δbwk+nwk

(−Rk+1[ak+1−bak+1]×21Δt)nwk+nwk+1

(−Rk+1[ak+1−bak+1]×21Δt)nwk+1+nak

(−Rk)nak+nak+1

(−Rk+1)nak+1(13)

把公式（13）的结论代入到公式（12）中，就可以得到关于位移的error-state的离散形势下的状态转移方程了。

速度的error-state

和位移同理：

δ v k + 1 = δ v k + { [ R k t ( a k t − b a k t ) − R k ( a k − b a k ) ] + [ R k + 1 t ( a k + 1 t − b a k t ) − R k + 1 ( a k + 1 − b a k ] } Δ t (14) \begin{aligned} \delta{v_{k+1}}&=\delta{v_k}+\{[R_k^t(a_k^t-b_{ak}^t)- R_k(a_k-b_{ak})]+[R_{k+1}^t(a_{k+1}^t-b_{ak}^t)-R_{k+1}(a_{k+1}-b_{ak}]\}\Delta{t} \end{aligned} \tag{14} δvk+1=δvk+{[Rkt(akt−bakt)−Rk(ak−bak)]+[Rk+1t(ak+1t−bakt)−Rk+1(ak+1−bak]}Δt(14)

可以看到，这次后面的部分和公式13是一模一样的，这里就不进行过多的推导了。

零偏Bias的error-state

同样的方式有：

b k + 1 t = b k + 1 + δ b k + 1 = b k t + n t Δ t = b k + n Δ t + δ b k + 1 = b k + δ b k + n t Δ t → δ b k + 1 = δ b k + ( n t − n ) ⏟ n b Δ t (15) \begin{aligned} b_{k+1}^t&=b_{k+1}+\delta{b_{k+1}}=b_{k}^t+n^t\Delta{t} \\ &=b_{k}+n\Delta{t}+\delta{b_{k+1}}=b_{k}+\delta{b_k}+n^t\Delta{t} \\ & \rightarrow \delta{b_{k+1}}=\delta{b_k}+\underbrace{(n^t-n)}_{n_b}\Delta{t} \end{aligned} \tag{15} bk+1t=bk+1+δbk+1=bkt+ntΔt=bk+nΔt+δbk+1=bk+δbk+ntΔt→δbk+1=δbk+nb

(nt−n)Δt(15)

整个状态转移方程的矩阵形式

结合上述的公式（11）（12）（14）（15），可以得到最终的状态转移方程的矩阵形式为下图，相比于之前的欧拉法复杂了很多：

VINS-Mono(一)——IMU预积分及系统初始化

visual-inertial alignment

这部分属于VINS中初始化阶段，我个人认为是一个不错的部分，因为VINS没有引入任何先验性的东西来初始化，而是真正的进行了各个量的估计，主要包括：

对陀螺仪初始零偏的估计；
对初始重力的估计；
对机体坐标系速度的估计；
对初始机体坐标系到世界坐标系的转移；

下面各个击破，这部分的前提是整个系统已经通过足够的激励得到了较好的IMU积分和Visual位姿估计（这些位姿没有尺度）。

对陀螺仪初始零偏的估计

这部分主要涉及到旋转，因此和尺度没有关系，因此作为第一步进行计算。

整体的思路为：

算法通过视觉SFM得到了一系列初始姿态 { q i ∣ i = 0 , . . . , N } \{q_i|i=0,...,N\} {qi∣i=0,...,N}；
同时预积分阶段得到了每两帧之间的相对姿态 { r j + 1 j ∣ j = 0 , . . . , N − 1 } \{r^j_{j+1}|j=0,...,N-1\} {rj+1j∣j=0,...,N−1}；
构建优化问题为：视觉帧间的相对姿态 q i + 1 i q_{i+1}^i qi+1i与IMU积分相对姿态 r j + 1 j r_{j+1}^j rj+1j的差的和最小，即：

E = ∣ ∣ e i + 1 i ∣ ∣ 2 = ∣ ∣ ( q i − 1 ⊗ q i + 1 ⏟ v i s u a l ) − 1 ⊗ r i i + 1 ⏟ I M U ∣ ∣ 2 (16) E=||e_{i+1}^i||^2=||(\underbrace{q_i^{-1}\otimes q_{i+1}}_{visual})^{-1} \otimes \underbrace{r_{i}^{i+1}}_{IMU} ||^2 \tag{16} E=∣∣ei+1i∣∣2=∣∣(visual

qi−1⊗qi+1)−1⊗IMU

rii+1∣∣2(16)

针对公式16，下面构建最小二乘问题有：

E = ∣ ∣ e i + 1 i ( b w ) ∣ ∣ 2 = ∣ ∣ [ 1 ϕ ( b w ) ] ∣ ∣ 2 where ϕ = 1 2 θ = 1 + ϕ ( b w ) T ϕ ( b w ) (17) \begin{aligned} E=||e_{i+1}^{i}(b_w)||^2&=||\begin{bmatrix}1 \\ \phi(b_w) \end{bmatrix} ||^2 \text{ where } \phi=\frac{1}{2}\theta \\ &=1+\phi(b_w)^T \phi(b_w) \end{aligned} \tag{17} E=∣∣ei+1i(bw)∣∣2=∣∣[1ϕ(bw)]∣∣2 where ϕ=21θ=1+ϕ(bw)Tϕ(bw)(17)

取 δ b w \delta{b_w} δbw的扰动之后有：

E = ∣ ∣ e i + 1 i ( b w + δ b w ) ∣ ∣ 2 = 1 + ( ϕ ( b w ) + J δ b w ) T ( ϕ ( b w ) + J δ b w ) where J = ∂ ϕ ∂ b w = ϕ + δ ϕ − ϕ δ b w = 1 2 δ θ δ b w = 1 2 J b w θ = 1 + ϕ ( b w ) T ϕ ( b w ) + ϕ ( b w ) T J δ b w + δ b w T J T ϕ ( b w ) + δ b w T J T J δ b w ∂ E ∂ δ b w = 0 → J T ϕ ( b w ) + J T J δ b w = 0 → 2 ( J b w θ ) T ϕ ( b w ) + ( J b w θ ) T ( J b w θ ) δ b w = 0 → δ b w = − ( ( J b w θ ) T ( J b w θ ) ) − 1 ( 2 ( J b w θ ) T ϕ ( b w ) ) (18) \begin{aligned} E=||e_{i+1}^{i}(b_w+\delta{b_w})||^2&=1+(\phi(b_w)+J\delta{b_w})^T(\phi(b_w)+J\delta{b_w}) \text{ where } J=\frac{\partial\phi}{\partial{b_w}}=\frac{\phi+\delta{\phi}-\phi}{\delta{b_w}}=\frac{1}{2}\frac{\delta{\theta}}{\delta{b_w}}=\frac{1}{2}J^{\theta}_{b_w} \\ &=1+\phi(b_w)^T\phi(b_w)+\phi(b_w)^TJ\delta{b_w}+\delta{b_w}^TJ^T\phi(b_w)+\delta{b_w}^TJ^TJ\delta{b_w} \\ \frac{\partial{E}}{\partial{\delta{b_w}}}=0 &\rightarrow J^T\phi(b_w)+J^TJ\delta{b_w}=0 \\ & \rightarrow 2(J^{\theta}_{b_w})^T\phi(b_w)+(J^{\theta}_{b_w})^T(J^{\theta}_{b_w})\delta{b_w}=0 \\ & \rightarrow \delta{b_w}=-((J^{\theta}_{b_w})^T(J^{\theta}_{b_w}))^{-1}(2(J^{\theta}_{b_w})^T\phi(b_w)) \tag{18} \end{aligned} E=∣∣ei+1i(bw+δbw)∣∣2∂δbw∂E=0=1+(ϕ(bw)+Jδbw)T(ϕ(bw)+Jδbw) where J=∂bw∂ϕ=δbwϕ+δϕ−ϕ=21δbwδθ=21Jbwθ=1+ϕ(bw)Tϕ(bw)+ϕ(bw)TJδbw+δbwTJTϕ(bw)+δbwTJTJδbw→JTϕ(bw)+JTJδbw=0→2(Jbwθ)Tϕ(bw)+(Jbwθ)T(Jbwθ)δbw=0→δbw=−((Jbwθ)T(Jbwθ))−1(2(Jbwθ)Tϕ(bw))(18)

这部分推导和代码中的基本上是一模一样了，唯一需要特别注意的是代码中的 ϕ \phi ϕ是IMU与visual的差，而这里的推导使用的是visual与IMU的差，所以差了一个负号（下面注释中的注意1），这里简单贴一下代码：

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    // 构建增量方程
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        // 注意1. 这个地方是对IMU的预积分取转置，根据四元数的转置的性质，后面的所有的相关变量都多了个负号
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    
    // 相当于对所有的状态求了一个公共的bias
    // 个人认为初始化部分的时间较短，所有共享一个bias是没有问题的
    delta_bg = A.ldlt().solve(b);
    ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
	
    // 重新预积分
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

Visual Inertial Alignment

经过了上一步骤之后，算法开始进行整个的视觉位姿和惯导位姿的对齐工作，算法在这个过程中主要求解的变量为每个预积分时间间隔的速度、整个视觉的尺度以及在视觉初始帧上的重力加速度（这个地方也是笔者认为VINS-Mono工作很细致且合理的地方，因为整个过程中算法并没有依靠任何的先验信息），求解的状态变量如下：

X I = [ v b 0 b 0 , v b 1 b 1 , ⋯ v b n b n , g c 0 , s ] \mathcal{X}_{I}=\left[\mathbf{v}_{b_{0}}^{b_{0}}, \mathbf{v}_{b_{1}}^{b_{1}}, \cdots \mathbf{v}_{b_{n}}^{b_{n}}, \mathbf{g}^{c_{0}}, s\right] XI=[vb0b0,vb1b1,⋯vbnbn,gc0,s]

还是先看算法的输入：

相机参考系 C 0 C_0 C0下的每个时刻的位姿 T c k c 0 = [ R c k c 0 ∣ p c k c 0 ] , k = 0 , . . . , N T^{c0}_{c_k}=[R^{c0}_{c_k}|p^{c0}_{c_k}],k=0,...,N Tckc0=[Rckc0∣pckc0],k=0,...,N；
IMU坐标系 b b b下的每个时间间隔的积分值 p b k + 1 b k , v b k + 1 b k , k = 0 , . . . , N − 1 {p_{bk+1}^{bk}, v_{bk+1}^{bk},k=0,...,N-1} pbk+1bk,vbk+1bk,k=0,...,N−1；

于是可以在相机的参考系 C 0 C_0 C0构建如下的模型：

{ s c 0 p b k + 1 = s c 0 p b k + c 0 v b k Δ t − 1 2 c 0 g Δ t 2 + c 0 p b k + 1 b k c 0 v b k + 1 = c 0 v b k − c 0 g Δ t + c 0 v b k + 1 b k (18) \begin{aligned} \begin{cases} s{^{c0}}p_{bk+1} &= s^{c0}p_{bk}+{^{c0}}v_{bk}\Delta{t}-\frac{1}{2}{^{c0}}g\Delta{t}^2+{^{c0}}p_{bk+1}^{bk} \\ {^{c0}}v_{bk+1} &= {^{c0}}v_{bk}-{^{c0}}g\Delta{t}+{^{c0}}v_{bk+1}^{bk} \end{cases} \end{aligned} \tag{18} {sc0pbk+1c0vbk+1=sc0pbk+c0vbkΔt−21c0gΔt2+c0pbk+1bk=c0vbk−c0gΔt+c0vbk+1bk(18)

在上述公式中，笔者特别把参考系写在了符号的左上角，例如 c 0 p b k ^{c0}p_{bk} c0pbk表示在 C 0 C_0 C0坐标系下，IMU在K时刻的位置；但是我们仅仅有K时刻Camera的位置，这个也很简单，基本上就是一个简单的三角转换关系，图形表示的话如下：

VINS-Mono(一)——IMU预积分及系统初始化

其中：

a = c 0 p c k a={^{c0}}p_{ck} a=c0pck， b = c 0 p b k c k b={^{c0}}p^{ck}_{bk} b=c0pbkck， c = c 0 p b k c={^{c0}}p_{bk} c=c0pbk；
那么c是需要求解的量，a是视觉得到的量，那么剩下的关键就是b如何得到了，其实也比较简单，因为通常情况下我们知道了IMU与Camera之间的外参 T c b = [ R c b ∣ t c b ] = [ R c k b k ∣ t c k b k ] T_{c}^{b}=[R_{c}^{b}|t_{c}^{b}]=[R_{ck}^{bk}|t_{ck}^{bk}] Tcb=[Rcb∣tcb]=[Rckbk∣tckbk]，那可以看到，外参对应的参考系是K时刻的IMU系，即 b k b_k bk，那转到 C 0 C_0 C0坐标系下也简单，直接转一下就OK了，于是有：

s c 0 p b k = s c 0 p c k + ( − R b k c 0 p c k b k ⏟ c 0 p c k b k ) = s c 0 p c k − R c k c 0 ( R c b ) T ⏟ R b k c 0 p c b (19) \begin{aligned} s{^{c0}}p_{bk}&=s{^{c0}}p_{ck}+(-\underbrace{R^{c0}_{bk}p_{ck}^{bk}}_{{^{c0}}p_{ck}^{bk}}) \\ &=s{^{c0}}p_{ck}-\underbrace{R^{c0}_{ck}(R_c^b)^T}_{R_{bk}^{c0}} p_{c}^{b} \end{aligned} \tag{19} sc0pbk=sc0pck+(−c0pckbk

Rbkc0pckbk)=sc0pck−Rbkc0

Rckc0(Rcb)Tpcb(19)

所以公式（18）可以进一步写作：

{ p b k + 1 b k = R c 0 b k [ s ( c 0 p b k + 1 − c 0 p b k ) − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] = R c 0 b k [ s c 0 p c k + 1 − R b k + 1 c 0 p c b − s c 0 p b k + R b k c 0 p c b − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] = R c 0 b k [ s ( c 0 p c k + 1 − c 0 p b k ) − R b k c 0 v b k Δ t + 1 2 c 0 g Δ t 2 ] − R b k + 1 b k p c b + p c b v b k + 1 b k = R c 0 b k ( c 0 v b k + 1 − c 0 v b k + c 0 g Δ t ) = R c 0 b k ( R b k + 1 c 0 v b k + 1 − R b k c 0 v b k + c 0 g Δ t ) (20) \begin{aligned} \begin{cases} p_{bk+1}^{bk} &= R_{c0}^{bk} \left[s({^{c0}}p_{bk+1}-{^{c0}}p_{bk})-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right] \\ &= R_{c0}^{bk} \left[s{^{c0}}p_{ck+1}-R_{bk+1}^{c0}p_{c}^{b}-s{^{c0}}p_{bk}+R_{bk}^{c0}p_{c}^{b}-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right]\\ &= R_{c0}^{bk} \left[s({^{c0}}p_{ck+1}-{^{c0}}p_{bk})-R_{bk}^{c0}v_{bk}\Delta{t}+\frac{1}{2}{^{c0}}g\Delta{t}^2\right]-R_{bk+1}^{bk}p_{c}^{b}+p_{c}^{b}\\ v_{bk+1}^{bk} &= R_{c0}^{bk}({^{c0}}v_{bk+1} - {^{c0}}v_{bk}+{^{c0}}g\Delta{t}) \\ &= R_{c0}^{bk}(R_{bk+1}^{c0}v_{bk+1} - R_{bk}^{c0}v_{bk}+{^{c0}}g\Delta{t}) \\ \end{cases} \end{aligned} \tag{20} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧pbk+1bkvbk+1bk=Rc0bk[s(c0pbk+1−c0pbk)−Rbkc0vbkΔt+21c0gΔt2]=Rc0bk[sc0pck+1−Rbk+1c0pcb−sc0pbk+Rbkc0pcb−Rbkc0vbkΔt+21c0gΔt2]=Rc0bk[s(c0pck+1−c0pbk)−Rbkc0vbkΔt+21c0gΔt2]−Rbk+1bkpcb+pcb=Rc0bk(c0vbk+1−c0vbk+c0gΔt)=Rc0bk(Rbk+1c0vbk+1−Rbkc0vbk+c0gΔt)(20)

公式（20）可以理解为一个观测方程，由状态变量 [ v b k , v b k + 1 , c 0 g , s ] \left[ v_{bk} , v_{bk+1}, {^{c0}}g, s \right] [vbk,vbk+1,c0g,s]得到K～K+1时刻IMU积分的值，所以写作矩阵的形式就是：

z ^ b k + 1 b k = [ α ^ b k + 1 b k − p c b + R c 0 b k R b k + 1 c 0 p c b β ^ b k + 1 b k ] = H b k + 1 b k X I + n b k + 1 b k (21) \hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}=\left[\begin{array}{c} \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}-\mathbf{p}_{c}^{b}+\mathbf{R}_{c_{0}}^{b_{k}} \mathbf{R}_{b_{k+1}}^{c_{0}} \mathbf{p}_{c}^{b} \\ \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}} \end{array}\right]=\mathbf{H}_{b_{k+1}}^{b_{k}} \mathcal{X}_{I}+\mathbf{n}_{b_{k+1}}^{b_{k}} \tag{21} z^bk+1bk=[α^bk+1bk−pcb+Rc0bkRbk+1c0pcbβ^bk+1bk]=Hbk+1bkXI+nbk+1bk(21)

其中：

H b k + 1 b k = [ − I Δ t k 0 1 2 R c 0 b k Δ t k 2 R c 0 b k ( p ‾ c k + 1 c 0 − p ‾ c k c 0 ) − I R c 0 b k R b k + 1 c 0 R c 0 b k Δ t k 0 ] \mathbf{H}_{b_{k+1}}^{b_{k}}=\left[\begin{array}{cccc} -\mathbf{I} \Delta t_{k} & \mathbf{0} & \frac{1}{2} \mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k}^{2} & \mathbf{R}_{c_{0}}^{b_{k}}\left(\overline{\mathbf{p}}_{c_{k+1}}^{c_{0}}-\overline{\mathbf{p}}_{c_{k}}^{c_{0}}\right) \\ -\mathbf{I} & \mathbf{R}_{c_{0}}^{b_{k}} \mathbf{R}_{b_{k+1}}^{c_{0}} & \mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k} & \mathbf{0} \end{array}\right] Hbk+1bk=[−IΔtk−I0Rc0bkRbk+1c021Rc0bkΔtk2Rc0bkΔtkRc0bk(pck+1c0−pckc0)0]

那么对于公式（21）的解法就比较多了，可以直接用线性方程的解，也可以用迭代的方式求解，这里就不赘述了。

需要特别注意的一点，在VINS-Mono的实现中，作者在尺度部分引入了一个100倍的系数，个人理解这里并不是权重，应该是怕最终解算出来的的尺度很小，因此想把这个数值变得大一些，防止因为数值问题而导致结果错误。

Refine Gravity

算法在获取了初始的重力加速度之后，又对重力加速度进行了细调，期望找到一个更准的重力加速度，这部分和上部分的理论基本一致，不过对于重力变量的优化，作者采用的是球面坐标的表示方法，论文中给出的图示如下：

VINS-Mono(一)——IMU预积分及系统初始化

其中：

∣ g ∣ |g| ∣g∣表示标准的重力加速度的模长， g → \overrightarrow{g} g

表示本次优化的初始重力方向的方向向量；
b 1 , b 2 b_1, b_2 b1,b2表示在Tangent平面上的两个正交向量，当做是优化空间的基底；

那么重写公式（18）有：

{ s c 0 p b k + 1 = s c 0 p b k + c 0 v b k Δ t − 1 2 ( c 0 g 0 + [ b 1 , b 2 ] [ w 1 w 2 ] ) Δ t 2 + c 0 p b k + 1 b k c 0 v b k + 1 = c 0 v b k − ( c 0 g 0 + [ b 1 , b 2 ] [ w 1 w 2 ] ) Δ t + c 0 v b k + 1 b k (22) \begin{aligned} \begin{cases} s{^{c0}}p_{bk+1} &= s^{c0}p_{bk}+{^{c0}}v_{bk}\Delta{t}-\frac{1}{2}({^{c0}}g_0+[b1, b2]\begin{bmatrix}w1 \\ w2\end{bmatrix})\Delta{t}^2+{^{c0}}p_{bk+1}^{bk} \\ {^{c0}}v_{bk+1} &= {^{c0}}v_{bk}-({^{c0}}g_0+[b1, b2]\begin{bmatrix}w1 \\ w2\end{bmatrix})\Delta{t}+{^{c0}}v_{bk+1}^{bk} \end{cases} \end{aligned} \tag{22} ⎩⎪⎪⎨⎪⎪⎧sc0pbk+1c0vbk+1=sc0pbk+c0vbkΔt−21(c0g0+[b1,b2][w1w2])Δt2+c0pbk+1bk=c0vbk−(c0g0+[b1,b2][w1w2])Δt+c0vbk+1bk(22)

于是公式（20）变作：

{ p b k + 1 b k = R c 0 b k [ s ( c 0 p c k + 1 − c 0 p b k ) − R b k c 0 v b k Δ t + [ b 1 , b 2 ] [ w 1 w 2 ] ] − R b k + 1 b k p c b + p c b + 1 2 R c 0 b k c 0 g 0 Δ t 2 v b k + 1 b k = R c 0 b k ( R b k + 1 c 0 v b k + 1 − R b k c 0 v b k + ( [ b 1 , b 2 ] [ w 1 w 2 ] ) Δ t ) + R c 0 b k c 0 g 0 Δ t (23) \begin{aligned} \begin{cases} p_{bk+1}^{bk} &= R_{c0}^{bk} \left[s({^{c0}}p_{ck+1}-{^{c0}}p_{bk})-R_{bk}^{c0}v_{bk}\Delta{t}+[b1, b2]\begin{bmatrix}w1 \\ w2\end{bmatrix}\right]-R_{bk+1}^{bk}p_{c}^{b}+p_{c}^{b}+\frac{1}{2}R_{c0}^{bk}{^{c0}}g_0\Delta{t}^2\\ v_{bk+1}^{bk} &= R_{c0}^{bk}(R_{bk+1}^{c0}v_{bk+1}-R_{bk}^{c0}v_{bk}+([b1, b2]\begin{bmatrix}w1 \\ w2\end{bmatrix})\Delta{t})+R_{c0}^{bk}{^{c0}}g_0\Delta{t} \end{cases} \end{aligned} \tag{23} ⎩⎪⎪⎨⎪⎪⎧pbk+1bkvbk+1bk=Rc0bk[s(c0pck+1−c0pbk)−Rbkc0vbkΔt+[b1,b2][w1w2]]−Rbk+1bkpcb+pcb+21Rc0bkc0g0Δt2=Rc0bk(Rbk+1c0vbk+1−Rbkc0vbk+([b1,b2][w1w2])Δt)+Rc0bkc0g0Δt(23)

这部分也可以写作线性方程的形式，这里就不赘述了。

VINS-Mono(一)——IMU预积分及系统初始化

文章目录

Reference

预积分的推导

ESKF

积分与预积分

VINS中的中点法（mid-point）

旋转的error-state

位移的error-state

速度的error-state

零偏Bias的error-state

整个状态转移方程的矩阵形式

visual-inertial alignment

对陀螺仪初始零偏的估计

Visual Inertial Alignment

Refine Gravity

继续阅读

ROS笔记：Ubuntu18.04环境下melodic源码安装gmapping

win10+VS2017配置Sophus库win10+VS2017配置Sophus库

学习笔记 -- opencv图像去畸变法一：使用 getOptimalNewCameraMatrix + initUndistortRectifyMap + remap 矫正图像法一完整代码：法二：使用 undistort（）法三：手动实现参考

解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 1 讲预备知识

代码中启动ros节点roslaunch和rosrun,代码启动节点与关闭节点

cartographer从入门到放弃4---核心算法分析---数据处理与Local SLAM

视觉slam线匹配算法

apollo自动驾驶平台配置笔记

粒子滤波学习记录粒子滤波学习记录

国内外LiDAR SLAM实验室总结☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

基于ORB特征点匹配的对极几何约束实现（源码+讲解）

GMS：基于网格运动统计的快速极度鲁棒的特征匹配摘要1 简介2 本文的方法3 应用于快速记分的网格框架4 实验 5 结论

在Ubuntu16.04上提取相邻序列图像之间的ORB的特征点，并用暴力方法找到匹配点并连线一、什么是ORB特征二、什么是暴力匹配三、实现代码四、运行方法

Turtlebot :" where am I? " -——amcl的原理

视觉传感器在无人驾驶领域取得技术突破

SLAM主要步骤