VINS-Mono理論推導1

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_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}$

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}$

3. 對公式2進行離散化
   $\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}$
   其中：

   • $R$表示從機體系到世界座標系的旋轉；
   • $a_m$表示IMU的測量減去重力加速度的值；

4. 將公式3寫作矩陣形式，得到kalman濾波的預測階段：
   $\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}$
   其中：

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

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

   • $\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的運動模型：
$\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}$
可以看到，在進行積分的時候，公式中不僅使用了各個時刻的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}$，所以剩下就是求解上圖中的黃色部分的變化了，這裏採用一階近似來表示這個變化，推導如下：
$\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}$

其中需要說明的幾個點爲：

• 偏導這裏採用了求導的原始定義；
• 相對姿態使用四元數進行表示，這裏使用旋轉向量來表示誤差量，這樣的好處就是能在$\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}} \tag{7}$

---

VINS中的中點法（mid-point）

在VINS-Mono的代碼中，作者使用的是中點法得到上面的公式4的，其實和參考【2】中的中點法不太一樣，這裏的中點法使用的是兩端點的平均，下面進行簡單的推導，先給出離散時間下的迭代步驟：
$\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}$

整個過程中稍微有些奇怪的是VINS在表示陀螺儀的真值的時候，使用的是$w_t=w_m-b_w-\delta{b_w}+n_w$，不過因爲噪聲是高斯白噪聲，加和減是一樣的，所以這裏按這樣推導也無妨。

旋轉的error-state

這裏先推導旋轉的誤差項是因爲後面的位移和速度會用到這部分結論，整體來說和上面的ESKF一樣，使用truth-state=normal-state+error-state的方法進行推導，這部分沿用微分的方式：
$\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}$

下面對其中的一些符號進行說明：

• $\delta{q}=\begin{bmatrix} 1 \\ \frac{1}{2}\delta{\theta} \end{bmatrix}$

• $\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}$

• $\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}$

至此，把上述的展開代入到公式9最後一行中中：
$\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}$
所以離散化之後：
$\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}$

位移的error-state

取公式8中的位移迭代公式，並使用truth-state=normal-state+error-state的方法進行推導，有：
$\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}$
把$p_{k+1}$的迭代公式代入之後有：
$\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}$
把後面的大括號中的部分設爲E，單獨拿出來化簡，有：
$\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}$
把公式（13）的結論代入到公式（12）中，就可以得到關於位移的error-state的離散形勢下的狀態轉移方程了。

速度的error-state

和位移同理：
$\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}$
可以看到，這次後面的部分和公式13是一模一樣的，這裏就不進行過多的推導了。

零偏Bias的error-state

同樣的方式有：
$\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}$

整個狀態轉移方程的矩陣形式

結合上述的公式（11）（12）（14）（15），可以得到最終的狀態轉移方程的矩陣形式爲下圖，相比於之前的歐拉法複雜了很多：

 

visual-inertial alignment

這部分屬於VINS中初始化階段，我個人認爲是一個不錯的部分，因爲VINS沒有引入任何先驗性的東西來初始化，而是真正的進行了各個量的估計，主要包括：

1. 對陀螺儀初始零偏的估計；
2. 對初始重力的估計；
3. 對機體座標系速度的估計；
4. 對初始機體座標系到世界座標系的轉移；

下面各個擊破，這部分的前提是整個系統已經通過足夠的激勵得到了較好的IMU積分和Visual位姿估計（這些位姿沒有尺度）。

---

對陀螺儀初始零偏的估計

這部分主要涉及到旋轉，因此和尺度沒有關係，因此作爲第一步進行計算。

整體的思路爲：

1. 算法通過視覺SFM得到了一系列初始姿態$\{q_i|i=0,...,N\}$；

2. 同時預積分階段得到了每兩幀之間的相對姿態$\{r^j_{j+1}|j=0,...,N-1\}$；

3. 構建優化問題爲：視覺幀間的相對姿態$q_{i+1}^i$與IMU積分相對姿態$r_{j+1}^j$的差的和最小，即：
   $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}$

針對公式16，下面構建最小二乘問題有：
$\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}$
取$\delta{b_w}$的擾動之後有：
$\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}$
這部分推導和代碼中的基本上是一模一樣了，唯一需要特別注意的是代碼中的$\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工作很細緻且合理的地方，因爲整個過程中算法並沒有依靠任何的先驗信息），求解的狀態變量如下：
$\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^{c0}_{c_k}=[R^{c0}_{c_k}|p^{c0}_{c_k}],k=0,...,N$；
• IMU座標系$b$下的每個時間間隔的積分值${p_{bk+1}^{bk}, v_{bk+1}^{bk},k=0,...,N-1}$；

於是可以在相機的參考系$C_0$構建如下的模型：
$\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}$
在上述公式中，筆者特別把參考系寫在了符號的左上角，例如$^{c0}p_{bk}$表示在$C_0$座標系下，IMU在K時刻的位置；但是我們僅僅有K時刻Camera的位置，這個也很簡單，基本上就是一個簡單的三角轉換關係，圖形表示的話如下：

其中：

• $a={^{c0}}p_{ck}$，$b={^{c0}}p^{ck}_{bk}$，$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{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}$

所以公式（18）可以進一步寫作：
$\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}$
公式（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{\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}$
其中：
$\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]$
那麼對於公式（21）的解法就比較多了，可以直接用線性方程的解，也可以用迭代的方式求解，這裏就不贅述了。

需要特別注意的一點，在VINS-Mono的實現中，作者在尺度部分引入了一個100倍的係數，個人理解這裏並不是權重，應該是怕最終解算出來的的尺度很小，因此想把這個數值變得大一些，防止因爲數值問題而導致結果錯誤。

 

---

Refine Gravity

算法在獲取了初始 的重力加速度之後，又對重力加速度進行了細調，期望找到一個更準的重力加速度，這部分和上部分的理論基本一致，不過對於重力變量的優化，作者採用的是球面座標的表示方法，論文中給出的圖示如下：

其中：

• $|g|$表示標準的重力加速度的模長，$\overrightarrow{g}$表示本次優化的初始重力方向的方向向量；
• $b_1, b_2$表示在Tangent平面上的兩個正交向量，當做是優化空間的基底；

那麼重寫公式（18）有：
$\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}$
於是公式（20）變作：
$\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}$
這部分也可以寫作線性方程的形式，這裏就不贅述了。