和單目純視覺的初始化只需要獲取R，t和feature的深度不同，VIO的初始化話通常需要標定出所有的關鍵參數，包括速度，重力方向，feature深度，以及相機IMU外參$R_{c}^{b}$和$p_{c}^{b}$。
一. 外參旋轉矩陣初始化
在Feature Detection and Tracking模塊中，利用Harris特征點匹配通過基礎矩陣和Ransac恢復出$R_{Ck+1}^{Ck}$；
相應的使用IMU陀螺儀數據積分得到$R_{bk+1}^{bk}$；
這兩個測量滿足：
$R_{bk+1}^{bk}\cdot R_{c}^{b} = R_{c}^{b}\cdot R_{Ck+1}^{Ck}$
將測量得到的$R$用四元數表示，並利用四元數的運算（左乘右乘），可以得到一個約束方程：
$Q_{k+1}^{k}\cdot q_{c}^{b} = 0$
利用多組數據可以獲得一個超定方程:
$Q_{N}\cdot q_{c}^{b} = 0$
通過求解最小二乘解，得到$q_{c}^{b}$。注意其中$Q_{N}$是包含使用Huber核的權重的，權重是通過殘差矩陣計算得到，權重通過計算IMU項和視覺findFundamentalMat計算得到的旋轉矩陣之間的差別得到，差別越大的$Q_{k+1}^{k}$，在$Q_{N}$中權重越小。
旋轉初始化成功判定依據：
如果IMU三軸充分旋轉，那么4*4矩陣$Q_{N}$的null space零空間的秩為1，可以得到穩定解；
如果旋轉退化，則$Q_{N}$零空間的秩大於1，使用SVD分解，判斷倒數第二小（第三大）的奇異值是否大於閾值。
因此，旋轉標定初始化的關鍵在有充分的三軸旋轉。
二. 速度，重力方向，feature深度，以及外參平移矩陣初始化
可以將$R_{c}^{b}$，$R_{bk+1}^{bk}$，$R_{bk+1}^{b0}$當做已知固定值，使用最大似然估計（Maximum Likelihood Estimation, MLE）和Mahalanobis范數（可以理解為帶權重的二范數（橢球向量范數））構建cost function，構建無偏的狀態估計問題。
在Sliding Window（例如選取5幀）中對測量值構建最小二乘問題：
VINS中feature沒有通過描述子匹配跟蹤，也不僅像ORB-SLAM中選取觀測次數多（跟蹤效果好）的特征點，另外加入了視角足夠大這個條件。雖然最后選取出的特征點數量比較少（150個左右），但是都是可觀性很好的特征點。
（一）IMU測量模型
構建目標函數需要對測量模型建模，IMU的測量模型和預積分一樣，預積分得到的測量值$\hat{z}$與狀態估計值之間的差值取Mahalanobis范數，其中權重為協方差矩陣的逆（信息矩陣）。
參考預積分公式：
得到IMU測量的誤差方程：
其中
為了求解高斯分布的最大似然，需要知道測量的協方差及其對應的狀態轉移方程（用於協方差的更新）
通過IMU的運動學方程微分，可以獲得狀態轉移方程
假設初始IMU協方差$P_{bk}^{bk}$為0，則協方差傳播方程為
協方差矩陣的傳播和卡爾曼濾波的預測過程中協方差的傳播是一樣的，從系統的微分運動方程獲取的。VINS中使用的是first-order discrete-time propagation，或者稱為forward Euler method，其中加速度計的噪聲$Q_{t}$需要預先知道或者使用imu_tk標定。
（二）視覺測量模型
重投影誤差在VIO中，通常需要經過IMU坐標系的變換；這樣有兩個好處：
- 將IMU的參數（外參）整合在視覺的residual中一起參與優化；
- IMU的坐標系在預積分中是已知的，不需要再去計算；
因此將i幀獲取的2D feature重投影到j幀上滿足觀測方程：
其中的坐標變換如下圖所示：
初始化收斂判定：
cost function的協方差矩陣中外參平移向量$p_{c}^{b}$對應的協方差值（SVD分解）小於設定閾值時，收斂，初始化完成。由於要獲得協方差矩陣（對應整個狀態向量，而不是每一塊誤差項），需要對信息矩陣求逆，計算量較大，因此會放在新的線程中去完成（這里似乎有可以優化的空間）。