里程計航跡推演與IMU預積分

參考：VINS代碼註釋版

預積分，難以理解的地方主要是，雅可比的計算、怎麼通過迭代的方式得到協方差的、使用deltaBias更新預積分是什麼鬼?
其實，這與計算輪式里程計在兩幀圖像之間的協方差過程類似，只不過里程計比較簡單，使用航跡推演，再結合協方差傳遞律，就能得到，所以就很容易明白了，里程計中的航跡推演就對應IMU預積分中的使用中值法計算得到的兩幀之間 PVQ 增量（注意，對速度和位移的預積分並沒有去掉加速度測量值中的重力g），即預積分要算的東西，預積分說白了就是測量值，測量值都有noise，航跡推演的noise我們知道怎麼算得，IMU預積分的noise也一樣，只是稍微麻煩一點，用的一套誤差運動學方程，最後可得到如下狀態方程，
$\dot x = Ax+Bw, w \sim N(0, \Sigma)$

因爲$A$ 和 $B$ 不是常矩陣，所以這是一個線性時變連續系統，學過控制理論的對這個公式應該很熟悉吧，因爲採樣週期很小，所以可以進行近似離散化，進而得到，
$x (k+1)= (I + \delta tA)x(k)+\delta t Bw(k), w(k) \sim N(0, \Sigma_k)$

注意，這裏的 $w(k)$ 已經變成了離散的噪聲，關於IMU的連續噪聲如何換成離散噪聲用過Kalibr的都比較熟悉吧。爲了得到從第i張圖片到第i+1張圖片之間對IMU預積分的協方差，協方差的傳播律大家已經很熟悉了，那預積分的協方差就是對上式進行迭代計算。除此之外，對IMU預積分的時候爲什麼要計算一個雅可比矩陣，因爲後面要在初值處進行泰勒展開（實際上只用到了對bias的雅克比）。每來一幀IMU數據，就遞推一次雅可比矩陣和協方差矩陣，最後處理完最後一幀IMU數據後，預積分就完成了，協方差有了，然後就順理成章開始優化了.
上面這塊的實現在 IntegrationBase::midPointIntegration() 函數內

預積分的結果其實就是測量值，類似我們相機觀測到的特徵點的座標、里程計的測量值等等，唯一的區別就是，預積分作爲測量值本來是不會改變的，但是因爲這個測量值裏面還含有優化變量bias，這是因爲，我們在預積分公式的推到時，將IMU相對世界座標系（w）的位姿、速度都轉換成相對上一幀圖像時IMU的座標系上去了（即，b_k），但是確沒有bias做任何處理，所以bias就留下了（其實bias本身變化就很小，兩幀圖像之間只有微小的變化，不像IMU的位姿，每一時刻都有很大程度的不同），既然這個預積分得到含有優化變量bias，那麼當bias改變了，我們自然就要重新計算預積分了，怎麼就算了？一個直接的方式就是使用新的bias重新計算一次預積分，這種方法固然可行，但是這和我們當初爲什麼分離優化變量的初衷相違背（我們正是爲了在優化變量更新後不重新計算預積分啊），那該怎麼辦呢，還記得這個bias的變化很小這個常識嗎，很自然就想到使用泰勒展開（進行近似），泰勒展開需要雅可比啊（即，預積分對 k 時刻bias的梯度的轉置），這個雅可比我們剛剛在預積分時就算過了啊，所以啊，預積分最後一層神祕的面紗已經揭開了^~^
上面這塊的實現在 IntegrationBase::evaluate() 函數內

剛說到要開始優化了，IMU預積分的結果就是測量值，我們如何使用使用測量值進行優化呢？還記得我們如何使用相機對地圖點的觀測構造殘差的的嗎，這裏也一樣，其實我們在推導預積分的時候就已經得到殘差了（公式5），只是我們沒有寫成殘差的形式而已，需要進行移項，然後結果一目瞭然（公式24），然後就是優化的套路，如高斯牛頓法，給一個增量，通過泰勒展開分離，得到這個增量的線性方程，然後就構成了一個線性最小二乘，通過求解這個線性最小二乘，使目標函數最小，也就得到這個增量應該是多少，將這個增量加到上一次優化變量的值上，就得到了本次的優化變量的值，一直循環下去，就得到最終的優化變量的結果，因爲進行泰勒展開時，需要使用雅可比，計算雅可比使用的是待優化變量的初值（爲了收斂，必須使用初值），只有第一次優化時，計算雅可比使用的是初值，之後使用的上一次優化結果作爲線性化點計算雅可比，因爲需要反覆更新雅可比矩陣，所以耗時是難免，那麼有沒有什麼好的方法避免呢？SVO爲了加速，採用inverse compositional formulation方式，只需要計算一次雅可比矩陣
上面這塊的實現在 IMUFactor::Evaluate() 函數內

---

<完>
@leatherwang

---