Reference

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

预积分的推导

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

ESKF

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

1. ESKF有两个变量，一个是normal-state $X$，一个是truth-state $X_t$，两者的关系如下：
   $${\mathbf{X}}_{\mathbf{t}}=\left[\begin{array}{c}{\mathbf{p}}_{\mathbf{t}}\\ {\mathbf{v}}_{\mathbf{t}}\\ {\mathbf{q}}_{\mathbf{t}}\\ {\mathbf{a}}_{bt}\\ {\mathbf{\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\\ {\mathbf{\omega }}_b+\delta {\omega }_b\end{array}\right]=\mathbf{X}⊞\delta \mathbf{X}\text{\hspace{1em}(1)}$$

2. 进一步，通过对两边求微分得到：
   $$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}& =-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b-\mathbf{R}{\mathbf{a}}_n\\ \dot{\delta \mathbf{\theta }}& =-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ \dot{\delta {\mathbf{a}}_b}& ={\mathbf{a}}_w\\ \delta {\dot{\mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\end{array}\text{\hspace{1em}(2)}$$

3. 对公式2进行离散化
   $$\begin{array}{rl}\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 \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b\right)\Delta t+{\mathbf{v}}_{\mathbf{i}}\\ \delta \mathbf{\theta }& \leftarrow I-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\Delta t\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b\Delta t+{\mathbf{\theta }}_{\mathbf{i}}\\ \delta {\mathbf{a}}_b& \leftarrow \delta {\mathbf{a}}_b+{\mathbf{a}}_{\mathbf{i}}\\ \delta {\mathbf{\omega }}_b& \leftarrow \delta {\mathbf{\omega }}_b+{\mathbf{\omega }}_{\mathbf{i}}\end{array}\text{\hspace{1em}(3)}$$
   其中：

   • $R$表示从机体系到世界坐标系的旋转；
   • $a_m$表示IMU的测量减去重力加速度的值；

4. 将公式3写作矩阵形式，得到kalman滤波的预测阶段：
   $$\begin{array}{l}\hat{\delta }\mathbf{x}\leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \hat{\mathbf{\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}\text{\hspace{1em}(4)}$$
   其中：

   • $${\mathbf{F}}_{\mathbf{x}}={\frac{\partial f}{\partial \delta \mathbf{x}}|}_{\mathbf{x},{\mathbf{u}}_m}=\left[\begin{array}{cccccc}\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[{\mathbf{\omega }}_m-{\mathbf{\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]$$

   • $${\mathbf{F}}_{\mathbf{i}}={\frac{\partial f}{\partial \mathbf{i}}|}_{\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]$$

   • $${\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]$$

积分与预积分

积分比较简单，拿到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˙=Rik​i0​​(am​−ba​−na​)q˙​=q⊗q{wm​−bw​−nw​}ba​˙​=wa​bw​˙​=ww​​(5)
可以看到，在进行积分的时候，公式中不仅使用了各个时刻的IMU测量值，而且用到零偏值，因此当零偏值变化的时候，整个微分数值就产生了变化，于是最后的相对状态值就会产生变化，此时一种方法是将修改后的零偏值带入到积分过程中得到新的相对位姿和速度。

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

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

图中X表示使用IMU数据进行积分的过程，IMU认为在这段时间内，坐标系的变化为从$I_k$到$I_{k+1}$，而当状态变量进行了优化，零偏变化$\delta b$时，积分过程需要加上$\Delta X$，即假如零偏变化了$\delta b$，那么IMU认为坐标系的变化应该到$I_{k+1}^{{}^{\prime }}$，所以剩下就是求解上图中的黄色部分的变化了，这里采用一阶近似来表示这个变化，推导如下：
$$\{\begin{array}{l}{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 \left[\begin{array}{c}1\\ \frac{1}{2}\frac{\partial \theta }{\partial b}\delta b\end{array}\right]=q_n\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\frac{\partial \theta }{\partial b}\delta b\end{array}\right]=q_n\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\frac{\theta +\delta \theta -\theta }{\delta b}\delta b\end{array}\right]=q_n\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\frac{\delta \theta }{\delta b}\delta b\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$

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

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

剩下的就是如何求解上述推导中的$\frac{\delta p}{\delta b},\frac{\delta v}{\delta b},\frac{\delta q}{\delta b}$了，特别注意的这里的三个导数和公式4中的变量并不一样，因为公式（4）是两个IMU周期之间的递推公式，而公式（6）是两个相机周期（一般都是N倍于IMU周期）之间的递推公式，那这个数值是多少呢？这里其实是一个连乘的操作，如下：
$$\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$$
所以为什么要用ESKF也就很明显了，同时使用ESKF进行error-state状态的更新还有另一个好处就是能够得到这次观测的协方差，如何得到呢？其实也很简单，使用协方差的原始定义就好啦，这里以位置为例：
$$P_p=E((p_t-p_{int}{)}^T(p_t-p_{int}))=E(\delta p^T\delta p)=P_{\delta p}\text{\hspace{1em}(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+21​2Rk​(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-\delta b_w+n_w$，不过因为噪声是高斯白噪声，加和减是一样的，所以这里按这样推导也无妨。

旋转的error-state

这里先推导旋转的误差项是因为后面的位移和速度会用到这部分结论，整体来说和上面的ESKF一样，使用truth-state=normal-state+error-state的方法进行推导，这部分沿用微分的方式：
$$\begin{array}{rl}\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{array}\text{\hspace{1em}(9)}$$

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

• $$\delta q=\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]$$

• $$\begin{array}{rl}[\Omega {]}_R& =\left[\begin{array}{cc}0& -{\Omega }^T\\ \Omega & -[\Omega {]}_{\times }\end{array}\right]\\ [\Omega {]}_L& =\left[\begin{array}{cc}0& -{\Omega }^T\\ \Omega & [\Omega {]}_{\times }\end{array}\right]\end{array}$$

• $$\begin{array}{rl}\Omega & =\frac{w_m^k+w_m^{k+1}}{2}-b_w^k\\ {\Omega }_t& =\frac{w_m^k+n_w^k+w_m^{k+1}+n_w^{k+1}}{2}-b_w^k-\delta b_w^k=\Omega -\delta b_w^k+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}\end{array}$$

至此，把上述的展开代入到公式9最后一行中中：
$$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \theta }\end{array}\right]& =\left[\begin{array}{cc}0& -({\Omega }_t-\Omega {)}^T\\ ({\Omega }_t-\Omega )& -([{\Omega }_t{]}_{\times }+[\Omega {]}_{\times })\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]\\ & =\left[\begin{array}{cc}0& -(-\delta b_w^k+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}{)}^T\\ -\delta b_w^k+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}& -(2[\Omega ]-[\delta {b_w^k}_{\times }+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}{]}_{\times })\end{array}\right]\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]\\ & =\left[\begin{array}{c}...\\ -[\Omega {]}_{\times }\delta \theta -\delta b_w^k+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}\end{array}\right]\\ \dot{\delta \theta }& \to -[\Omega {]}_{\times }\delta \theta -\delta b_w^k+\frac{1}{2}{n}_w^k+\frac{1}{2}{n}_w^{k+1}\end{array}\text{\hspace{1em}(10)}$$
所以离散化之后：
$$\delta {\theta }_{k+1}=\delta {\theta }_k-[\Omega {]}_{\times }\Delta t\delta {\theta }_k-\delta b_w^k\Delta t+\frac{1}{2}{n}_w^k\Delta t+\frac{1}{2}{n}_w^{k+1}\Delta t\text{\hspace{1em}(11)}$$

位移的error-state

取公式8中的位移迭代公式，并使用truth-state=normal-state+error-state的方法进行推导，有：
$$\begin{array}{rl}{p}_{k+1}^t& =p_{k+1}+\delta p_{k+1}=p_k^t+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{array}$$
把$p_{k+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，单独拿出来化简，有：
$$\begin{array}{rl}E& =R_{k}Exp(\delta {\theta }_k)(a_k-n_a^k-b_{ak}-\delta b_{ak})-R_k(a_k-b_{ak})\\ & +R_{k+1}Exp(\delta {\theta }_{k+1})(a_{k+1}-n_a^{k+1}-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_a^k-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_a^{k+1}-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_w^k\Delta t+\frac{1}{2}{n}_w^k\Delta t+\frac{1}{2}{n}_w^{k+1}\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_w^k\Delta t+\frac{1}{2}{n}_w^k\Delta t+\frac{1}{2}{n}_w^{k+1}\Delta t)\\ & \text{where }\dot{\delta b_a}=0\to \delta b_{ak+1}=\delta b_{ak}\\ & \\ & \approx \underset{\delta \theta }{\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}}\\ & +\underset{b_{ak}}{\underbrace{(-R_k+R_{k+1})\delta b_{ak}}}+\underset{b_{wk}}{\underbrace{(R_{k+1}[a_{k+1}-b_{ak+1}{]}_{\times }\Delta t)\delta b_{wk}}}\\ & +\underset{n_w^k}{\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}{]}_{\times }\frac{1}{2}\Delta t)n_w^k}}+\underset{n_w^{k+1}}{\underbrace{(-R_{k+1}[a_{k+1}-b_{ak+1}{]}_{\times }\frac{1}{2}\Delta t)n_w^{k+1}}}\\ & +\underset{n_a^k}{\underbrace{(-R_k)n_a^k}}+\underset{n_a^{k+1}}{\underbrace{(-R_{k+1})n_a^{k+1}}}\end{array}\text{\hspace{1em}(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

同样的方式有：
$$\begin{array}{rl}{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\\ & \to \delta b_{k+1}=\delta b_k+\underset{n_b}{\underbrace{(n^t-n)}}\Delta t\end{array}\text{\hspace{1em}(15)}$$

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

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

visual-inertial alignment

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

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

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

对陀螺仪初始零偏的估计

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

整体的思路为：

1. 算法通过视觉SFM得到了一系列初始姿态 { q i ∣ i = 0 , . . . , N } \{q_i|i=0,...,N\} {qi​∣i=0,...,N}；

2. 同时预积分阶段得到了每两帧之间的相对姿态 { r j + 1 j ∣ j = 0 , . . . , N − 1 } \{r^j_{j+1}|j=0,...,N-1\} {rj+1j​∣j=0,...,N−1}；

3. 构建优化问题为：视觉帧间的相对姿态$q_{i+1}^i$与IMU积分相对姿态$r_{j+1}^j$的差的和最小，即：
   $$E=||e_{i+1}^i|{|}^2=||(\underset{visual}{\underbrace{q_i^{-1}\otimes q_{i+1}}}{)}^{-1}\otimes \underset{IMU}{\underbrace{r_i^{i+1}}}|{|}^2\text{\hspace{1em}(16)}$$

针对公式16，下面构建最小二乘问题有：
$$\begin{array}{rl}E=||e_{i+1}^i(b_w)|{|}^2& =||\left[\begin{array}{c}1\\ \varphi (b_w)\end{array}\right]|{|}^2\text{ where }\varphi =\frac{1}{2}\theta \\ & =1+\varphi (b_w{)}^T\varphi (b_w)\end{array}\text{\hspace{1em}(17)}$$
取$\delta b_w$的扰动之后有：
$$\begin{array}{rl}E=||e_{i+1}^i(b_w+\delta b_w)|{|}^2& =1+(\varphi (b_w)+J\delta b_w{)}^T(\varphi (b_w)+J\delta b_w)\text{ where }J=\frac{\partial \varphi }{\partial b_w}=\frac{\varphi +\delta \varphi -\varphi }{\delta b_w}=\frac{1}{2}\frac{\delta \theta }{\delta b_w}=\frac{1}{2}{J}_{b_w}^{\theta }\\ & =1+\varphi (b_w{)}^T\varphi (b_w)+\varphi (b_w{)}^{T}J\delta b_w+\delta {b_w}^T{J}^T\varphi (b_w)+\delta {b_w}^T{J}^{T}J\delta b_w\\ \frac{\partial E}{\partial \delta b_w}=0& \to J^T\varphi (b_w)+J^{T}J\delta b_w=0\\ & \to 2(J_{b_w}^{\theta }{)}^T\varphi (b_w)+(J_{b_w}^{\theta }{)}^T(J_{b_w}^{\theta })\delta b_w=0\\ & \to \delta b_w=-((J_{b_w}^{\theta }{)}^T(J_{b_w}^{\theta }){)}^{-1}(2(J_{b_w}^{\theta }{)}^T\varphi (b_w))\end{array}\text{\hspace{1em}(18)}$$
这部分推导和代码中的基本上是一模一样了，唯一需要特别注意的是代码中的$\varphi$是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工作很细致且合理的地方，因为整个过程中算法并没有依靠任何的先验信息），求解的状态变量如下：
$${\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]$$

还是先看算法的输入：

• 相机参考系$C_0$下的每个时刻的位姿$T_{c_k}^{c0}=[R_{c_k}^{c0}|p_{c_k}^{c0}],k=0,...,N$；
• IMU坐标系$b$下的每个时间间隔的积分值$p_{bk+1}^{bk},v_{bk+1}^{bk},k=0,...,N-1$；

于是可以在相机的参考系$C_0$构建如下的模型：
$$\begin{array}{r}\{\begin{array}{ll}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{array}\end{array}\text{\hspace{1em}(18)}$$
在上述公式中，笔者特别把参考系写在了符号的左上角，例如${}^{c0}{p}_{bk}$表示在$C_0$坐标系下，IMU在K时刻的位置；但是我们仅仅有K时刻Camera的位置，这个也很简单，基本上就是一个简单的三角转换关系，图形表示的话如下：

其中：

• $a={}^{c0}{p}_{ck}$，$b={}^{c0}{p}_{bk}^{ck}$，$c={}^{c0}{p}_{bk}$；

• 那么c是需要求解的量，a是视觉得到的量，那么剩下的关键就是b如何得到了，其实也比较简单，因为通常情况下我们知道了IMU与Camera之间的外参$T_c^b=[R_c^b|t_c^b]=[R_{ck}^{bk}|t_{ck}^{bk}]$，那可以看到，外参对应的参考系是K时刻的IMU系，即$b_k$，那转到$C_0$坐标系下也简单，直接转一下就OK了，于是有：
  $$\begin{array}{rl}s{}^{c0}{p}_{bk}& =s{}^{c0}{p}_{ck}+(-\underset{{}^{c0}{p}_{ck}^{bk}}{\underbrace{R_{bk}^{c0}{p}_{ck}^{bk}}})\\ & =s{}^{c0}{p}_{ck}-\underset{R_{bk}^{c0}}{\underbrace{R_{ck}^{c0}(R_c^b{)}^T}}{p}_c^b\end{array}\text{\hspace{1em}(19)}$$

所以公式（18）可以进一步写作：
$$\begin{array}{r}\{\begin{array}{ll}{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{array}\end{array}\text{\hspace{1em}(20)}$$
公式（20）可以理解为一个观测方程，由状态变量$\left[v_{bk},v_{bk+1},{}^{c0}g,s\right]$得到K～K+1时刻IMU积分的值，所以写作矩阵的形式就是：
$${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\mathbf{\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{\mathbf{\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}\text{\hspace{1em}(21)}$$
其中：
$${\mathbf{H}}_{b_{k+1}}^{b_k}=\left[\begin{array}{cccc}-\mathbf{I}\Delta t_k& 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& 0\end{array}\right]$$
那么对于公式（21）的解法就比较多了，可以直接用线性方程的解，也可以用迭代的方式求解，这里就不赘述了。