IMU的預計分算法

1. IMU的測量值是什么？

IMU的測量值是三軸加速度和三軸角速度。它們都是在IMU當前時刻的坐標系下表達的向量值，記作\(a_t, \omega_t\)

2.通過IMU的測量我們想獲得什么？

很顯然，我們要獲得的是當前時刻的位姿，包括位移\(P\)和旋轉\(R\)（如果用四元數表示就是\(q\)）。為了代碼實現中計算上的方便我們同時還保留了計算\(P\)時的中間量，當前時刻的速度\(V\)。它們也是緊耦合模型中待優化狀態變量的一部分

3. IMU的測量模型

\[\begin{align*} \hat{a}_t &= a_t+b_a+R_w^t g^w +n_a\\ \hat{\omega}_t&=\omega_t+b_{\omega}+n_{\omega} \end{align*}\tag{1} \]

其中\(\hat{a},\hat{\omega}\)分別是加速度和角速度的測量值；\(a,\omega\)分別是加速度和角速度的真實值；\(b_a,b_{\omega}\)分別是加速度和角速度的零偏（bias），它們用隨機游走模型（布朗運動）來建模；\(n_a,n_{\omega}\)是噪聲，它們用高斯白噪聲建模；\(g^w\)是世界坐標系下的重力加速度（的反向，當IMU靜止的時候，它測量到的是一個反向的重力加速度）；\(R_w^t\)是當前時刻的姿態的逆，將\(g^w\)從世界坐標系轉換到IMU坐標系

4. 狀態量的運動模型

由\(b_k\)時刻的狀態量得到\(b_{k+1}\)時刻的狀態量，通過簡單的運動學公式就可以得到。下面的公式中忽略了噪聲的影響：

\[\begin{align*} P_{b_{k+1}}^w&=P_{b_k}^w+V_{b_k}^w\Delta t+\frac{1}{2}\iint_{t\in[k,k+1]}R_t^w(\hat{a}_t-b_{a_t})-g^wdt^2\\ V_{b_{k+1}}^w&=V_{b_k}^w+\int_{t\in[k,k+1]}R_t^w(\hat{a}_t-b_{a_t})-g^wdt\\ q_{b_{k+1}}^w&=q_{b_k}^w\otimes\int_{t\in[k,k+1]}\frac{1}{2}\Omega(\hat{\omega}_t-b_{\omega_t})q_t^{b_k}dt \end{align*}\tag{2} \]

5.為什么要預積分，怎么預積分以及什么是預積分？

很奇怪把什么是預積分是什么放在最后介紹。一般的介紹順序都是先介紹什么，再介紹怎么，最后再介紹為什么。我們的順序剛好反了過來。因為預積分就是一系列操作過程的一個名字，不先介紹完怎么做很難說什么是預積分。

先說為什么：

我們知道在緊耦合中待優化的狀態變量是\(P,R(q),V,b_a,b_{\omega}\)，在優化的一次次迭代中這些變量是在不停變化的。由公式（2）可以看到當\(b_k\)時刻的狀態發生改變的時候，我們需要一次次的積分來求取\(b_{k+1}\)時刻的狀態，這個是很耗費計算資源的，為了節省計算資源和時間，我們希望能夠避免積分部分的重復計算。

怎么辦呢？

先忽略零偏的影響。觀察一下各個積分部分。首先\(q\)的積分部分很簡單，它僅與角速度的測量值有關，注意\(q_t^{b_k}\)與狀態量是無關的（不包括零偏），因為它是在\(b_k\)坐標系下的。\(P\)和\(V\)的積分部分除了與加速度測量值有關外還有一個\(R_t^w\)，它是世界坐標系下的，與\(R_{b_k}^w\)有關：\(q_t^w=q_{b_k}^w\otimes\int_{\tau\in[k,t]}\frac{1}{2}\Omega(\hat{\omega}_{\tau}-b_{\omega_\tau})q_\tau^{b_k}d\tau\)。假如在（2）的三個公式兩邊同時乘以\(q_{b_k}^w\)的逆\(q_w^{b_k}\)（或者\(R_w^{b_k}\))那么積分部分就僅與IMU的測量值（還有零偏，但暫時忽略）有關了。所以我們有

\[\begin{align*} R_w^{b_k}P_{b_{k+1}}^w&=R_w^{b_k}(P_{b_k}^w+V_{b_k}^w\Delta t - \frac{1}{2}g^w\Delta t^2) +\alpha_{b_{k+1}}^{b_k}\\ R_w^{b_k}V_{b_{k+1}}^w&=R_w^{b_k}(V_{b_k}^w-g^w\Delta t)+\beta_{b_{k+1}}^{b_k}\\ q_w^{b_k}q_{b_{k+1}^w}&=\gamma_{b_{k+1}}^{b_k} \end{align*}\tag{3} \]

其中：

\[\begin{align*} \alpha_{b_{k+1}}^{b_k}&=\iint_{t\in[k,k+1]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt^2\\ \beta_{b_{k+1}}^{b_k}&=\int_{t\in[k,k+1]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt\\ \gamma_{b_{k+1}}^{b_k}&=\int_{t\in[k,k+1]}\frac{1}{2}\Omega(\hat{\omega}_t-b_{\omega_t})\gamma_t^{b_k}dt \end{align*}\tag{4} \]

被稱為預積分。

預積分是什么？

所以預積分其實是加速度和角速度引起的\(b_{k+1}\)時刻的狀態量關於\(b_k\)時刻的增量。
所謂的預積分，就是預先積分好，之后需要的時候拿來用。我們知道IMU測量的是線性加速度和角速度。狀態量中速度是關於加速度的積分，位置是關於加速度的二次積分，姿態是關於角速度的積分。因此，在當前坐標系下（因為IMU的測量值就是在當前坐標系下的）把IMU測量值積分好，當需要的是，只需要根據情況進行坐標系轉換和加減了。
可以看到預積分與待優化的狀態量（忽略零偏）無關，它是一個固定的值。當優化過程中狀態量改變時（世界坐標系下的值）只需要將它們乘上\(R_{b_k}^w\)並代入（2）中就可以了。這樣重復的積分運算就簡化為了乘法運算，大大節省了計算量。

預積分的數值實現，歐拉法和中值積分法

（待完成）

IMUFactor的殘差和雅克比

殘差

IMU Factor是用IMU的測量來約束系統狀態\([P_i, V_i, Q_i,ba_i, bg_i]^T\)。殘差定義的是\(r =[\delta \alpha, \delta \beta, \delta \gamma, \delta b_a, \delta b_g]^T\)， 也就是ESKF中的誤差狀態。由公式(3)，我們可以通過系統狀態定義：

\[z^{b_k}_{b_{k+1}} = \left[\begin{matrix} R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1\over 2}g^w\Delta t^2 - V^w_{b_k}\Delta t) \\ R^{b_k}_w(V^w_{b_{k+1}}+g^w\Delta t -V^w_{b_k}) \\ {Q^{w}_{b_k}}^{-1} \otimes Q^w_{b_{k+1}}\\ {b_a}_{b_{k+1}} - {b_a}_{b_{k}} \\ {b_g}_{b_{k+1}} - {b_g}_{b_{k}} \end{matrix} \right]\]

由IMU的測量我們可以定義：

\[\hat{z}^{b_k}_{b_{k+1}} = \left[ \begin{matrix} \hat{\alpha}^{b_k}_{b_{k+1}}\\ \hat{\beta}^{b_k}_{b_{k+1}}\\ \hat{\gamma}^{b_k}_{b_{k+1}}\\ 0\\ 0 \end{matrix}\right]\]

殘差定義為：\(r = z^{b_k}_{b_{k+1}} - \hat{z}^{b_k}_{b_{k+1}}\)。零偏的殘差項是認為兩幀直接的零偏相差極小。

零偏對預積分的影響

前面我們討論預積分的時候忽略了零偏的影響，但零偏也是待優化的狀態量，每次優化迭代的時候它的值都會發生變化，進而影響了預積分的值，那么對於零偏怎么處理呢？這里分為兩種策略：
1）如果零偏變化很大，那么就老老實實去做積分，再做一次傳播，當然在代碼的實現中是把新的\(b_a,b_{\omega}\)代入（4）來計算新的預積分，以備之后使用，世界坐標系下的狀態量還是通過乘上\(R_{b_k}^w\)代入（2）獲得。
2）如果零偏變化較小，因為\(\alpha,\beta,\gamma\)都是關於零偏的非線性函數，將它們線性化(一階泰勒展開)，求出預積分的近似值來代替傳播。這就需要分別求\(\alpha,\beta,\gamma\)關於\(b_a,b_{\omega}\)的雅克比。

所以在單目初始化的時候，解算出新的\(b_g\)之后就調用了preIntegration->rePropagate()。而在IntegrationBase::Evaluate()中計算\(b_a, b_g\)改變對預積分的影響就用雅克比乘以\(\delta b_g, \delta b_g\)：

        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;

協方差矩陣的傳播

在優化項中，殘差是通過馬氏距離表達的：\(r^T \Sigma^{-1}r\)。因此這里就需要殘差\(\delta z^{b_k}_{b_{k+1}}\)的協方差矩陣。要得到協方差就需要分析噪聲的來源。這里我們參考ESKF中的分析方法，分為真值(True Value)，標稱值或測量值(Nominal Value)和誤差狀態(Error State)。一般情況下有： \(True Value = Nominal Value - Noise\)
。但是ESKF中從另一個角度來考慮問題：它把標稱值當作大信號，把誤差狀態當做小信號，所以定義了： \(True Value = Nominal Value + Error State\) 。因此有：\(Error State = - Noise = True Value - Nominal Value\)。根據我們上面對殘差項的定義（系統值減去測量值），殘差中的前三項其實就相當於Error State，而不是Noise。
這樣我們就來分析誤差狀態的協方差矩陣。

協方差矩陣的數值形式

雅克比

這里涉及到兩種雅克比矩陣：一種是IMUFactor中的，殘差關於系統狀態的雅克比；另一種是IMU預積分中的\(\delta z^{b_k}_{b_{k+1}}\)關於\(\delta z^{b_k}_{b_k}\)的雅克比。涉及到關於零偏的雅克比，后一種是前一種推導的基礎，或者說用后一種可以快速的推導出前一種。

\[\begin{split}\alpha^{b_k}_{b_{k+1}}& = &\hat{\alpha}^{b_k}_{b_{k+1}} + J^{\alpha}_{{b_a}_k} \delta {b_a}_k + J^{\alpha}_{{b_g}_k} \delta {b_g}_k \\ \alpha^{b_k}_{b_{k+1}} - \hat{\alpha}^{b_k}_{b_{k+1}} &=& J^{\alpha}_{{b_a}_k} \delta {b_a}_k + J^{\alpha}_{{b_g}_k} \delta {b_g}_k \\ {\partial \delta \alpha^{b_k}_{b_{k+1}} \over \partial \delta {b_a}_k} &= &J^{\alpha}_{{b_a}_k} \end{split}\]

\[\begin{split}{\partial r \over \partial b_{a_k}} & = &{\partial R^{b_k}_w(P^w_{b_{k+1}} - P^w_{b_k} + {1\over 2}g^w\Delta t^2 - V^w_{b_k}\Delta t) - \hat{\alpha}^{b_k}_{b_{k+1}}\over \partial b_{a_k}} \\ & = & -{\partial \alpha^{b_k}_{b_{k+1}} \over \partial b_{a_k}}\\ & = & -{\partial \delta \alpha^{b_k}_{b_{k+1}} \over \partial \delta {b_a}_k} \end{split}\]

\({\partial \delta \alpha^{b_k}_{b_{k+1}} \over \partial \delta {b_a}_k}\)由IntegrationBase::midPointIntergration()遞推的求出。

\[J_{t+\delta t} = (I+F_t \delta t)J_t, \qquad J_{b_k} = I \]