一、IMU的模型

以t時刻，IMU作為參考系，我們能夠得到以下的IMU模型：

左邊是IMU的測量值，包括了3軸的加速度，以及3軸的角速度資訊；右邊第一項是陀螺儀和加速度計的真值，第二項是偏移偏移，

最後一項是測量噪聲項。注意一點的是，由於IMU測量的時候，是除了重力加速度外，所有外力對IMU參考系力的和對系統產

生的加速度，會把反向重力加速度測量在內。意思是，當IMU自由落體的時候，加速度的讀數是0；IMU平放在桌子上的時候，讀數是g。

這裡假設噪聲 na,nb 是服從高斯正態分佈，而 ba,bw 偏置服從隨機遊走模型(倒數服從高斯正態分佈)。

在已知 初始的P1,Q1,V1,ba1,bg1 的情況下，並且知道每個時刻ba,bg的時候，通過下面的這個式子的離散化表示，

我們就能夠通過積分得到後面這每一個時刻的P, Q, V：

即可以通過這個式子：

正常這個公式是可以比較直觀上理解的；但是有一個問題，因為這個公式中，有一個 R旋轉矩陣是從imu座標系旋轉到世界

座標系的絕對位姿。後端優化的過程中，會不斷的修正 p,q,v 的值，使得一個代價函式最小，現在我們一旦修改了

一個p,  因為積分項裡面有一項與IMU的絕對位姿有關，因此我們又要進行重複的一個積分運算。即：

理解：首先要關注問題的關鍵，即使我們之前沒有接觸過預積分，但是我們看了上面的一段話，也要明白其實這個問題的本質是座標系的轉換問題；

想一下，假如我們從 1  到 2時刻 進行積分，然後後端優化，這時候我們可以求出來 p,q,v 優化後的值，那不能求出來就不管了吧，我們要對之前的

位姿態修正呀，可是你這一修正，q變了，對應的R變了，那就從新從1 積分到 2，得到一個準確的2對應的位置；OK，我們在往前走，2 到 3， R右變了，

在回來重新積分一下，得到準確的3，就這樣往復著，很麻煩！

 

為了避免這一個重複積分運算的過程，vins提出了預積分的概念，通過了座標系的轉換，將絕對的座標姿態轉化為了相對的座標轉換，把要積分的

項轉化了一個形式。即把R的絕對位姿轉化成相對位姿，相對於每一幀的絕對位姿，即：

現在三個臨時量只和當前的加速度計偏移和陀螺儀的偏移有關。而於他們在世界座標系的絕對位姿沒有任何關係。任何幀的P，V，Q的改變

不影響積分過程，因此就不用一直重複積分。這三個變數只和偏移ba,bw有關。更新偏移，由於偏移變化是隨時間緩慢變化的，因此可以用泰勒一階展開：

而通過離散積分的方式，可以獲取這三個積分項的離散迭代表達：

 

到這裡，我們就可以帶入IMU 6個軸的觀測值，進行迭代求解，最終解算出這三個積分項；這裡的預積分裡面還是有IMU偏差的，

通常是和視覺聯合初始化，可以求出來，補償一下就行了，更新預積分的值！而不用像以前一樣更新P VQ；得到更新後的預積分

值後，疊加到以前的PVQ，在轉換到世界座標系下！

到這裡，我們還不知道這三個積分項的含義是什麼，直觀上理解，可以認為，bk幀做自由落體運動，

以這個幀作為參考系，imu在bk到bk+1這段時間的位移，速度，姿態的變化。從數學上去描述就是：

上面這個預積分的值是我們用公式 推算出來的，代表的意思就是b座標系下， K+1 時刻PVQ 減去  K 時刻PVQ ；

我們還有一個預積分的測量值，如上面的（7）；因此imu的殘差vins就定義為：

殘差是一個向量，最終的殘差是一個標量，因此我們就要把向量轉化為一個標量。即要乘以他的協方差矩陣。協方差矩陣可以理解為，

因為向量中每個元素變化的情況值不一樣，有些元素變化得比較大（方差大），有些元素變化比較小（方差小），為了使得把這種變化

統一起來，因此需要有一個標準來量化它，就認為是這個協方差矩陣。為了獲取協方差矩陣，需要建立一個線性高斯誤差狀態傳播方程，

由線性高斯系統的協方差，就可以推匯出方程協方差矩陣了，也就是測量狀態的協方差矩陣了。

噪聲對角線協方差矩陣就是我們需要標定imu的四個引數。兩個測量噪聲的方差，兩個偏置倒數的方差。

得到這個殘差項的目的是幹嗎？當然是為了求解待優化的變化量了，然後補償，修正；繼續下一步的推算、測量、優化....

這個優化的方法常用的有EKF，或者 LM等；

 

總結預積分：

為了使得非線性優化中，不用每次改變位姿後，積分需要重新計算的問題。
現在改變位姿（速度，位移，旋轉四元數關於世界座標系的），預積分內的項保持不變。
改變ba,bw後，預積分的項可以通過泰勒一階展開來進行更新。