VIN2.IMU預積分

0.引言

搬運工+自己閱讀代碼理解。

• 參考說明

pipeline如圖：

本節則是閱讀總結IMU(100hz)-->IMU Pre-integration部分。processIMU()函數、integration_base.h-->IntegrationBase類。

1.IMU測量模型

忽略地球旋轉,IMU 測量方程爲：
$\begin{array}{c}{a_{t}=a_{t(r e a l)}+b_{a_{t}}+R_{w}^{t} g^{w}+n_{a}} \\ {w_{t}=w_{t(r e a l)}+b_{w_{t}}+n_{w}}\end{array} \tag{1}$上標 $g$ 表示gyro,$a$ 表示acc,$w$表示在世界座標系world,$b$ 表示imu機體座標系body.imu的真實值爲 ω, a 測量值爲$\omega$、$a$;測量值爲$\tilde{\omega}, \tilde{\mathbf{a}}$。

IMU 的測量信息是在body座標系(即 IMU 座標)中獲取，假設噪聲$n_{a}$和$n_{w}$服從高斯分佈:

$\begin{aligned} n_{a} & \sim N\left(0, \sigma_{a}^{2}\right) \\ n_{w} & \sim N\left(0, \sigma_{w}^{2}\right) \end{aligned} \tag{2}$
加速度偏置$b_{a_{t}}$和陀螺儀偏置$b_{w_{t}}$被建模爲隨機遊走:
$\dot{b}_{a_{i}}=n_{b_{a}}$$\dot{b}_{w_{i}}=n_{b_{w}}\tag{3}$其中$n_{b_{a}}$和$n_{b_{w}}$服從高斯分佈:
$\begin{aligned} n_{b_{a}} & \sim N\left(0, \sigma_{b_{a}}^{2}\right) \\ n_{b_{w}} & \sim N\left(0, \sigma_{b_{w}}^{2}\right) \end{aligned}\tag{4}$
既是其導數服從高斯分佈！

2.當前時刻 PVQ 的連續形式

對於連續兩個關鍵幀 $b_{k}$ 和$b_{k+1}$,它們對應的時刻分別爲 $t_{k}, t_{k+1}$ 。可以根據 $\left[t_{k}, t_{k+1}\right]$時間間隔內 IMU 的測量值,對系統的位置、速度和旋轉等狀態進行傳播(注意這裏的四元數採用了實部在後,虛部在前的形式):
$p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(a_{t}-b_{a_{i}}-n_{a}\right)-g^{w}\right) d t^{2}\tag{5}$$v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(a_{t}-b_{a_{i}}-n_{a}\right)-g^{w}\right) d t \tag{6}$$q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} q_{t}^{b_{k}} \otimes\left(w_{t}-b_{w_{i}}-n_{w}\right) d t$$=q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(w_{t}-b_{w_{t}}-n_{w}\right) q_{t}^{b_{k}} d t \tag{7}$其中：$\Omega(w)=\left[\begin{array}{cc}{-[w]_{ \times}} & {w} \\ {-w^{T}} & {0}\end{array}\right],[w]_{ \times}=\left[\begin{array}{ccc}{0} & {-w_{z}} & {w_{y}} \\ {w_{z}} & {0} & {-w_{x}} \\ {-w_{y}} & {w_{x}} & {0}\end{array}\right] \tag{8}$$\Delta t_{k}$是$\left[t_{k}, t_{k+1}\right]$之間的時間間隔,$R_{t}^{w}$爲 $t$ 時刻從body座標系到世界座標系的旋轉矩陣,$q_{t}^{b_{k}}$爲用四元數表示的 $t$ 時刻從body座標系到世界座標系的旋轉。$a_t,w_t$爲 IMU 測量的加速度和角速度,是在 Body 自身座標系,world 座標系是IMU 所在的慣導系。

3.當前時刻 PVQ 的中值法離散形式

前面給出的是連續時刻的相機當前 PVQ 的迭代公式,基於中值法的公式 , 即從第 i 個 IMU 時刻 到第 i+1 個 IMU 時刻的積分過程 ,這與Estimator::processIMU()函數中的 Ps[j]、Rs[j]和 Vs[j]是一致的(代碼中的 j 時刻即爲此處的i+1),IMU 積分出來的第 j 時刻的物理量可以作爲第 j 幀圖像的初始值。
$p_{b_{i+1}}^{w}=p_{b_{i}}^{w}+v_{b_{i}}^{w} \delta t+\frac{1}{2} \overline{{a}}_{i} \delta t^{2} \tag{9}$$v_{b_{i+1}}^{w}=v_{b_{i}}^{w}+\overline{{a}}_{i} \delta t\tag{10}$$q_{b_{i+1}}^{w}=q_{b_{i}}^{w} \otimes\left[\frac{1}{2} \overline{{\omega}}_{i} \delta t\right]\tag{11}$其中:$\overline{{a}}_{i}=\frac{1}{2}\left[q_{i}\left({a}_{i}-b_{a_{i}}\right)-g^{w}+q_{i+1}\left({a}_{i+1}-b_{a_{i}}\right)-g^{w}\right]\tag{12}$$\overline{{\omega}}_{i}=\frac{1}{2}\left({\omega}_{i}+{\omega}_{i+1}\right)-b_{\omega_{i}}\tag{13}$
與代碼中Estimator::processIMU():一一對應

		~ ~ ~
        int j = frame_count;
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;

若採用歐拉積分，其中：
$\mathbf{a}=\mathbf{q}_{w b_{k}}\left(\mathbf{a}^{b_{k}}-\left(\mathbf{b}_{k}^{a}\right)-\mathbf{g}^{w}\right.\tag{14}$$\omega=\omega^{b_{k}}-\mathbf{b}_{k}^{g} \tag{15}$這兩個等式表達式表達與上面不統一，不影響理解。

4.兩幀之間 PVQ 增量的連續形式

IMU的數據頻率一般遠高於視覺，在視覺兩幀k，k+1之間通常會有>10組IMU數據。IMU的數據通過積分，可以獲取當前位姿（p位置，q四元數表達的姿態）、瞬時速度等參數。
　　在VIO中，如果參考世界座標系對IMU進行積分，積分項中包含相對於世界座標系的瞬時旋轉矩陣，這樣有幾個問題：

1. 相對世界座標系的旋轉矩陣有drift，如果一直以其爲基準進行積分，必然造成積分誤差累積；
2. 在進行優化位姿調整時（通常是調整視覺KeyFrame的pose），相對於世界座標系的pose會變化，因而優化後的瞬時旋轉矩陣和積分時不同，那麼積分自然也就存在問題；
3. 一般這個旋轉矩陣不知道。。。

因此，一般的預積分的參考座標系爲k幀的IMU參考系，這樣可以解決以上問題：

1. 相對k幀的IMU進行積分，不會有累積誤差；
2. 即使後面調整了位姿，相對位置不變，因此預積分不存在問題；
3. 這個旋轉矩陣爲單位矩陣E，後面每出現一個IMU數據，都可以用任何一種數值積分的方法計算；同時可以將重力加速度提取到積分號外面不參加積分，相當於在重力參考系中積分，計算量也會減少。

注意這裏是求的增量，即是在窗口優化過程中的產生的變化$\Delta$.優化狀態向量包括滑動窗口內的所有相機狀態（位置P、旋轉Q、速度V、加速度偏置ba、陀螺儀偏置bw）、相機到IMU的外參、所有3D點的逆深度：
$\mathcal{X}=\left[\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots \mathbf{x}_{n}, \mathbf{x}_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right]$$\mathbf{x}_{k}=\left[\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}\right], k \in[0, n]$$\mathbf{x}_{c}^{b}=\left[\mathbf{p}_{c}^{b}, \mathbf{q}_{c}^{b}\right]$
故$\mathbf{q}_{b_{k}}^{w}$隨着優化是在改變的，導致公式(5)(6)(7)的積分隨着優化需重複計算，耗時耗力，故將積分項轉換到body座標系計算，減少重複。
.　　通過公式(5)(6)(7)可知,IMU 的預積分需要依賴與第 k 幀的 v 和 R,當我們在後端進行非線性優化時,需要迭代更新第 k 幀的 v 和 R,這將導致我們需要根據每次迭代後值重新進行積分,這將非常耗時。因此,我們考慮將優化變量從第 k 幀到第 k+1 幀的 IMU 預積分項b中分離開來,通過對公式(1)左右兩側各乘$R_{w}^{b_{k}}$ ,可化簡爲:

　　從積分式可以看出,系統位置、速度和旋轉等狀態的傳播需要關鍵幀$b_{k}$ 時刻的位置$p_{b_{k}}^{w}$ 、速度$\nu_{b_{k}}^{w}$和旋轉$q_{b_{k}}^{w}$,當這些起始狀態發生改變時,就需要按照上訴積分式重新進行狀態傳播。在基於優化的算法中,每個關鍵幀時刻的狀態需要頻繁調整,所以就需要頻繁地重新積分,這樣會浪費大量的計算資源。IMU 預積分就是爲了避免這種計算資源上的浪費。考慮將優化變量從第 k 幀到第 k+1 幀的 IMU 預積分項中分離開來,通過對公式(5)(6)(7)左右兩側各乘$R_{w}^{b_{k}}$ ,可化簡爲公式(16)(17)(18)(19)(20)(21).
　　IMU 預積分的思路簡單來說,就是將參考座標系從世界座標系 $w$調整爲第 $k$ 個關鍵幀時刻的body座標系 $b_k$ :

　　$R_{w}^{b_{k}} p_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}} \tag{16}$$R_{w}^{b_{k}} v_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(v_{b_{k}}^{w}-g^{w} \Delta t_{k}\right)+\beta_{b_{k+1}}^{b_{k}} \tag{17}$$q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}} \tag{18}$其中：
$\alpha_{b_{k+1}}^{b_{k}}=\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{b_{k}}\left(a_{t}-b_{a_{i}}-n_{a}\right)\right) d t^{2} \tag{19}$$\beta_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{b_{k}}\left(a_{t}-b_{a_{t}}-n_{a}\right)\right) d t \tag{20}$$\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(w_{t}-b_{w_{i}}-n_{w}\right) \gamma_{t}^{b_{k}} d t \tag{21}$ 　　新的積分項(12.13.14)中參考座標系變成了$b_{k}$ ,可以理解爲這時的積分結果爲$b_{k+1}$對於$b_{k}$ 的相對運動量,即使在優化過程中對關鍵幀的位置、速度和旋轉等狀態進行調整,也不對新的積分項產生任何影響,從而避免了重複積分。（從world系轉換爲body系）

這裏需要重新討論下公式(19)~(21)的預積分公式,以${\alpha}_{b_{k+1}}^{b_{k}}$爲例,發現它是與 IMU 的bias 相關的,而 bias 也是我們需要優化的變量,這將導致的問題是,當每次迭代時,我們得到一個新的 bias,又得根據公式中重新對第 k 幀和第 k+1 幀之間的 IMU 預積分,非常耗時。這裏假設預積分的變化量與 bias 是線性關係,可以寫成:
$\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a}+J_{b_{\omega}}^{\alpha} \delta b_{\omega}$$\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a}+J_{b_{\omega}}^{\beta} \delta b_{\omega}\tag{22}$$\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{\omega}}^{\gamma} \delta b_{\omega}}\end{array}\right]$其中的Jacobi矩陣後面講。

5.兩幀之間 PVQ 增量的離散形式

歐拉法：
下面給出離散時刻的 IMU 預積分公式,首先按照論文中採用的歐拉法,給出第 i 個 IMU時刻與第 i+1 個 IMU 時刻的變量關係爲:
$\hat{\alpha}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t^{2}$$\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t \tag{23}$$\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2}\left(\widehat{\omega}_{i}-b_{\omega_{i}}\right) \delta t}\end{array}\right]$
中值法：
代碼中採用的基於中值法的 IMU 預積分公式,這與 Estimator::processIMU()函數中的 IntegrationBase::push_back()上是一致的。特別注意這裏跟公式(9)~(13)是不一樣的,這裏積分出來的是前後兩幀之間的 IMU 增量信息,而公式(9)~(13)給出的當前幀時刻的物理量信息。這裏相當於在求解優化過程中產生的增量，計算的是增量值。

$\widehat{\alpha}_{i+1}^{b_{k}}=\widehat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \overline{\widehat{a}}_{i}^{\prime} \delta t^{2}$$\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+\overline{\widehat{a}}_{i}^{\prime} \delta t \tag{24}$$\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \overline{\omega}_{i}^{\prime} \delta t}\end{array}\right]$其中：$\overline{\hat{a}}_{i}^{\prime}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right]$$\overline{\widehat{\omega}}_{i}^{\prime}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}\tag{25}$

對應代碼integration_base.h--> midPointIntegration():

        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         

6.連續形式下 PVQ 增量的誤差、協方差及 Jacobian

.　　目前已經完成了IMU預積分測量值的梳理，而要將IMU預積分運用到非線性優化中，需要建立線性高斯誤差狀態遞推方程，由線性高斯系統的協方差，推導方程協方差矩陣，並求解對應的雅可比矩陣。IMU 在每一個時刻積分出來的值都有誤差,下面對誤差進行分析。預積分誤差：一段時間內 IMU 構建的預積分量作爲測量值,對兩時刻之間的狀態量進行約束,

$\left[\begin{array}{l}{\mathbf{r}_{p}} \\ {\mathbf{r}_{q}} \\ {\mathbf{r}_{v}} \\ {\mathbf{r}_{b a}} \\ {\mathbf{r}_{b g}}\end{array}\right]_{15 \times 1}=$$\left[\begin{array}{c}{\mathbf{q}_{b_{i} w}\left(\mathbf{p}_{w b_{j}}-\mathbf{p}_{w b_{i}}-\mathbf{v}_{i}^{w} \Delta t+\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}\right)-\boldsymbol{\alpha}_{b_{i} b_{j}}} \\ {2\left[\mathbf{q}_{b_{j}} b_{i} \otimes\left(\mathbf{q}_{b_{i} w} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z}} \\ {\mathbf{q}_{b_{i} w}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\boldsymbol{\beta}_{b_{i} b_{j}}} \\ {\mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a}} \\ {\mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g}}\end{array}\right]$

上面誤差中位移,速度,偏置都是直接相減得到。第二項是關於四元數的旋轉誤差,其中$[\cdot]_{x y z}$表示只取四元數的虛部$(x, y, z)$ 組成的三維向量。

問題的提出：一個 IMU 數據作爲測量值的噪聲方差我們能夠標定。現在,一段時間內多個 IMU 數據積分形成的預積分量的方差呢?由高斯分佈的線性組合性質：
$\mathbf{y}=\mathbf{A} \mathbf{x}, \mathbf{x} \in \mathcal{N}\left(0, \boldsymbol{\Sigma}_{x}\right)$則有：
$\boldsymbol{\Sigma}_{y}=A \boldsymbol{\Sigma}_{x} A^{\top}$
所以,要推導預積分量的協方差,我們需要知道 imu 噪聲和預積分量之間的線性遞推關係。類似於狀態轉移，回顧一下之前的卡爾曼濾波．

假設已知了相鄰時刻誤差的線性傳遞方程:
$\boldsymbol{\eta}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\eta}_{i k-1}+\mathbf{G}_{k-1} \mathbf{n}_{k-1}$比如:狀態量誤差爲$\boldsymbol{\eta}_{i k}=\left[\delta \boldsymbol{\theta}_{i k}, \delta \mathbf{v}_{i k}, \delta \mathbf{p}_{i k}\right]$測量噪聲爲$\mathbf{n}_{k}=\left[\mathbf{n}_{k}^{g}, \mathbf{n}_{k}^{a}\right]$.誤差的傳遞由兩部分組成:當前時刻的誤差傳遞給下一時刻,當前時刻測量噪聲傳遞給下一時刻。

協方差矩陣可以通過遞推計算得到:
$\boldsymbol{\Sigma}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\Sigma}_{i k-1} \mathbf{F}_{k-1}^{\top}+\mathbf{G}_{k-1} \mathbf{\Sigma}_{\mathbf{n}} \mathbf{G}_{k-1}^{\top}$其中,$\boldsymbol{\Sigma}_{\mathbf{n}}$是測量噪聲的協方差矩陣,方差從 i 時刻開始進行遞推,$\boldsymbol{\Sigma}_{i i}=\mathbf{0}$。

6.1.基於誤差隨時間變化的遞推方程

此處藉助卡爾曼濾波過程理解。通常對於狀態量之間的遞推關係是非線性的方程如$\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)$,其中狀態量爲 x,u 爲系統的輸入量。可以用兩種方法來推導狀態誤差傳遞的線性遞推關係:

• 一種是基於一階泰勒展開的誤差遞推方程。
• 一種是基於誤差隨時間變化的遞推方程。

非線性系統$\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)$的狀態誤差的線性遞推關係如下:
$\delta \mathbf{x}_{k}=\mathbf{F} \delta \mathbf{x}_{k-1}+\mathbf{G} \mathbf{n}_{k-1} \tag{26}$基於泰勒展開的誤差傳遞應用於 EKF 的協方差預測。論文中採用基於誤差隨時間變化的遞推方程。如果我們能夠推導狀態誤差隨時間變化的導數關係,比如:
$\dot{\delta \mathbf{x}}=\mathbf{A} \delta \mathbf{x}+\mathbf{B} \mathbf{n}$則誤差狀態的傳遞方程爲:
$\begin{aligned} \delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+\dot{\delta} \mathbf{x}_{k-1} \Delta t \\ \rightarrow \delta \mathbf{x}_{k} &=(\mathbf{I}+\mathbf{A} \Delta t) \delta \mathbf{x}_{k-1}+\mathbf{B} \Delta t \mathbf{n}_{k-1} \end{aligned}$可以看出有等式(26)中:
$\mathbf{F}=\mathbf{I}+\mathbf{A} \Delta t, \mathbf{G}=\mathbf{B} \Delta t$

回顧一下預積分的誤差遞推公式,將測量噪聲也考慮進模型:

$\mathbf{q}_{b_{i} b_{k+1}}=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \omega \delta t}\end{array}\right]$$\boldsymbol{\alpha}_{b_{i} b_{k+1}}=\boldsymbol{\alpha}_{b_{i} b_{k}}+\beta_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2}$$\beta_{b_{i} b_{k+1}}=\beta_{b_{i} b_{k}}+\mathbf{a} \delta t$$\begin{aligned} \mathbf{b}_{k+1}^{a} &=\mathbf{b}_{k}^{a}+\mathbf{n}_{\mathbf{b}_{k}^{a}} \delta t \\ \mathbf{b}_{k+1}^{g} &=\mathbf{b}_{k}^{g}+\mathbf{n}_{\mathbf{b}_{k}^{g}} \delta t \end{aligned}$其中：
$\boldsymbol{\omega}=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}+\mathbf{n}_{k}^{g}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}+\mathbf{n}_{k+1}^{g}-\mathbf{b}_{k}^{g}\right)\right)$$\mathbf{a}=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}+\mathbf{n}_{k}^{a}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}+\mathbf{n}_{k+1}^{a}-\mathbf{b}_{k}^{a}\right)\right)$確定誤差傳遞的狀態量,噪聲量,然後開始構建傳遞方程。

推導出如下的形式，直接給出推導結果：
$\left[\begin{array}{c}{\delta \boldsymbol{\alpha}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \boldsymbol{\beta}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \mathbf{b}_{k+1}^{a}} \\ {\delta \mathbf{b}_{k+1}^{g}}\end{array}\right]=\mathbf{F}\left[\begin{array}{c}{\delta \boldsymbol{\alpha}_{b_{k} b_{k}^{\prime}}} \\ {\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\ {\delta \boldsymbol{\beta}_{b_{k} b_{k}^{\prime}}} \\ {\delta \mathbf{b}_{k}^{a}} \\ {\delta \mathbf{b}_{k}^{g}}\end{array}\right]+\mathbf{G}\left[\begin{array}{c}{\mathbf{n}_{k}^{a}} \\ {\mathbf{n}_{k}^{g}} \\ {\mathbf{n}_{k}^{a}} \\ {\mathbf{n}_{k+1}^{a}} \\ {\mathbf{n}_{\mathbf{b}_{k}^{g}}} \\ {\mathbf{n}_{\mathbf{b}_{k}^{g}}}\end{array}\right] \tag{27}$

$\mathbf{F}, \mathbf{G}$爲兩個時刻間的協方差傳遞矩陣:

$\mathbf{F}=$$\left[\begin{array}{cccccc}{\mathbf{I}} & {\mathbf{f}_{12}} & {\mathbf{I} \delta t} & {-\frac{1}{4}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t^{2}} & {\mathbf{f}_{15}} \\ {\mathbf{0}} & {\mathbf{I}-[\boldsymbol{\omega}]_{ \times}} & {\mathbf{0}} & {\mathbf{0}} & {-\mathbf{I} \delta t} \\ {\mathbf{0}} & {\mathbf{f}_{32}} & {\mathbf{I}} & {-\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t} & {\mathbf{f}_{35}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}}\end{array}\right]$

$\mathbf{G}=$$\left[\begin{array}{cccccc}{\frac{1}{4} \mathbf{q}_{b_{i} b_{k}} \delta t^{2}} & {\mathbf{g}_{12}} & {\frac{1}{4} \mathbf{q}_{b_{i} b_{k+1}} \delta t^{2}} & {\mathbf{g}_{14}} & {\mathbf{0}} & {\mathbf{0}} \\ {\mathbf{0}} & {\frac{1}{2} \mathbf{I} \delta t} & {\mathbf{0}} & {\frac{1}{2} \mathbf{I} \delta t} & {\mathbf{0}} & {\mathbf{0}} \\ {\frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \delta t} & {\mathbf{g}_{32}} & {\frac{1}{2} \mathbf{q}_{b_{i} b_{k+1}} \delta t} & {\mathbf{g}_{34}} & {\mathbf{0}} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I} \delta t} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I} \delta t}\end{array}\right]$
其中：
$\mathbf{f}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{ \times} \delta t^{2}+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times\left(\mathbf{I}-[\boldsymbol{\omega}]_{ \times} \delta t\right) \delta t^{2}\right)$

$\mathbf{f}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{ \times} \delta t^{2}+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times\left(\mathbf{I}-[\omega]_{ \times} \delta t\right) \delta t^{2}\right)$

$\mathbf{f}_{15}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)(-\delta t)$

$\mathbf{f}_{35}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t\right)(-\delta t)$

$\mathbf{g}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{14}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times \delta t^{2}\right)\left(\frac{1}{2} \delta t\right)$

$\mathbf{g}_{32}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{34}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times \delta t^{2}\right)\left(\frac{1}{2} \delta t\right)$

代碼中$\mathbf{F}, \mathbf{G}$爲$\mathbf{F}, \mathbf{V}$。

這裏對公式(26)(27)的 IMU 誤差運動方程(狀態方程)再說明,將上式和 EKF 對比可知,上式恰好給出瞭如 EKF 一般對非線性系統線性化的過程,這裏的意義是表示下一個時刻的 IMU 測量誤差與上一個時刻的成線性關係,這樣我們根據當前時刻的值,可以預測出下一個時刻的均值和協方差,而公式(26)給出的是均值預測,協方差預測公式如下(對比卡爾曼濾波的第二個公式):
$P_{t+\delta t}^{b_{k}}=\left(\mathrm{I}+F_{t} \delta t\right) P_{t}^{b_{k}}\left(\mathrm{I}+F_{t} \delta t\right)^{T}+\left(G_{t} \delta t\right) Q\left(G_{t} \delta t\right)^{T}$其中：$F=\mathrm{I}+F_{t} \delta t, \quad V=G_{t} \delta t$上式給出了協方差的迭代公式,初始值$P_{b_{k}}^{b_{k}}=0$。其中,$Q$ 爲表示噪聲項的對角協方差矩陣:$Q^{12 \times 12}=\left[\begin{array}{cccc}{\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end{array}\right]$誤差項的 Jacobian 的迭代公式:$J_{t+\delta t}=\left(\mathrm{I}+F_{t} \delta t\right) J_{t}$其中 Jacobian 的初始值爲$J_{b_{k}}=I$

7.離散形式的 PVQ 增量誤差分析

首先直接給出 PVQ 增量誤差在離散形式下的矩陣形式,爲了與代碼一致,我們修改下變量順序,這和代碼中 midPointIntegration()函數是一致的。(但不知爲何計算的 V 中與前四個噪聲項相關的差個負號?)

$\left[\begin{array}{c}{\delta \alpha_{k+1}} \\ {\delta \theta_{k+1}} \\ {\delta \beta_{k+1}} \\ {\delta b_{k+1}} \\ {\delta b_{w_{k+1}}}\end{array}\right]=\left[\begin{array}{ccccc}{I} & {f_{01}} & {\delta t} & {f_{03}} & {f_{04}} \\ {0} & {f_{11}} & {0} & {0} & {-\delta t} \\ {0} & {f_{21}} & {I} & {f_{23}} & {f_{24}} \\ {0} & {0} & {0} & {I} & {0} \\ {0} & {0} & {0} & {0} & {I}\end{array}\right]\left[\begin{array}{c}{\delta \alpha_{k}} \\ {\delta \theta_{k}} \\ {\delta \beta_{k}} \\ {\delta b_{a_{k}}} \\ {\delta b_{w_{k}}}\end{array}\right]$
$+\left[\begin{array}{ccccc}{v_{00}} & {v_{01}} & {v_{02}} & {v_{03}} & {0} \\ {0} & {\frac{-\delta t}{2}} & {0} & {\frac{-\delta t}{2}} & {0} \\ {-\frac{R_{k} \delta t}{2}} & {v_{21}} & {-\frac{R_{k+1} \delta t}{2}} & {v_{23}} & {0} \\ {0} & {0} & {0} & {0} & {\delta t} & {0} \\ {0}& {0} & {0} & {0} & {0} & {\delta t}\end{array}\right]\left[\begin{array}{c}{n_{a_{k}}} \\ {n_{w_{k}}} \\ {n_{a_{k+1}}} \\ {n_{b_{a}}} \\ {n_{b_{w}}}\end{array}\right]\tag{28}$
其中：
$f_{01}=\frac{\delta t}{2} f_{21}=-\frac{1}{4} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t^{2}-\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t^{2}$

$f_{03}=-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t^{2}$

$f_{04}=\frac{\delta t}{2} f_{24}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{3}$

$f_{11}=I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t$
$f_{21}=-\frac{1}{2} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t-\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t$
$f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t$

$f_{24}=\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}$

$v_{00}=-\frac{1}{4} R_{k} \delta t^{2}$

$v_{01}=v_{03}=\frac{\delta t}{2} v_{21}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} \frac{\delta t}{2}$

$v_{02}=-\frac{1}{4} R_{k+1} \delta t^{2}$

$v_{21}=v_{23}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}$

8.離散形式的 PVQ 增量誤差的 Jacobian 和協方差

將公式(28)簡寫爲:

$\delta z_{k+1}^{15 \times 1}=F^{15 \times 15} \delta z_{k}^{15 \times 1}+V^{15 \times 18} Q^{18 \times 1}$
則 $Jacobian$ 的迭代公式爲:
$J_{k+1}^{15 \times 15}=F J_{k} \tag{29}$
其中,$Jacobian$ 的初始值爲$J_k = I$。這裏計算出來的$J_{k+1}$ 只是爲了給後面提供對 bias 的
$Jacobian$。

協方差的迭代公式爲:
$P_{k+1}^{15 \times 15}=F P_{k} F^{T}+V Q V^{T} \tag{30}$
其中,初始值$P_{k}=0$。$Q$ 爲表示噪聲項的對角協方差矩陣:
$Q^{18 \times 18}=\left[\begin{array}{cccccc}{\sigma_{a}^{2}} & {0} & {0} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} & {0} & {0} \\ {0} & {0} & {\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {0} & {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end{array}\right]$

代碼integration_base.h-->midPointIntegration():

    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)
    {
        //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;
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        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、V矩陣
            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;

            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;
            jacobian = F * jacobian;  \\公式（29）
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();\\公式（30）
        }

    }

總述：基於滑動窗口的 VIO Bundle Adjustment，總的loss funcution如下所示：

$\min _{\mathcal{X}} \underbrace{\rho\left(\left\|\mathbf{r}_{p}-\mathbf{J}_{p} \mathcal{X}\right\|_{\Sigma_{p}}^{2}\right)}_{\text { prior }}+\underbrace{\sum_{i \in B} \rho\left(\left\|\mathbf{r}_{b}\left(\mathbf{z}_{b_{i} b_{i+1}}, \mathcal{X}\right)\right\|_{\Sigma_{b_{i} b_{i+1}}}^{2}\right)}_{\text { IMU error }}$$+\underbrace{\sum_{(i, j) \in F} \rho\left(\left\|\mathbf{r}_{f}\left(\mathbf{z}_{\mathbf{f}_{j}}^{c_{i}}, \mathcal{X}\right)\right\|_{\Sigma_{\mathbf{f}_{j}}^{c_{i}}}^{2}\right)}_{\text { image error }}$

故，6、7、8小節實際是在求的IMU error項將會用到的量，爲後端優化做準備！