從兩幀IMU數據中獲得當前位姿的預測思路非常簡單，無非是求出當前時刻\(t\)與下一時刻\(t+1\)加速度的均值， 把它作為\(\Delta t\)時間內的平均加速度，有了這個平均加速度及當前時刻的初始速度和初始位置，就可以近似的求出\(t+1\)時刻的速度和位置。求出當前時刻\(t\)與下一時刻\(t+1\)角速度的均值， 把它作為\(\Delta t\)時間內的平均角速度，有了這個平均角速度及當前時刻的姿態，就可以近似的求出\(t+1\)時刻的姿態。
但是由於IMU的數據存在着坐標系、bias和重力加速度的問題需要額外的一些處理。
首先對於加速度，因為imu的加速度數據是在Body坐標系下表示的，所以要利用對應時刻的姿態將其轉換到世界坐標系下，轉換之前要減去bias，轉化之后要減去重力加速度（世界坐標系下的重力加速度恆等於9.8）：
\[a_{t,w}=Q_t(a_{t,b}-B_a)-g\\ a_{t+1,w}=Q_{t+1}(a_{t+1,b}-B_a)-g \]
\(Q_{t+1}\)是\(t+1\)時刻的姿態，需要用角速度的數據來近似計算：
\[\omega_t^{'} = \frac{1}{2}(\omega_t+\omega_{t+1})-B_g\\ Q_{t+1}=Q_t(\omega_t^{'}\Delta t) \]
有了\(t,t+1\)時刻的加速度，就可以求出\(t+1\)時刻的速度和位置：
\[a_{t,w}^{'}=\frac{1}{2}(a_{t,w}+a_{t+1,w})\\ V_{t+1}=V_t+a_{t,w}^{'}\Delta t\\ P_{t+1}=P_t+ V_t\Delta t + \frac{1}{2}a_{t,w}^{'}\Delta t ^2 \]
按照計算的順序整理一下整個積分的流程：
\[a_{t,w}=Q_t(a_{t,b}-B_a)-g\\ \omega_t^{'} = \frac{1}{2}(\omega_t+\omega_{t+1})-B_g\\ Q_{t+1}=Q_t(\omega_t^{'}\Delta t)\\ a_{t+1,w}=Q_{t+1}(a_{t+1,b}-B_a)-g\\ a_{t,w}^{'}=\frac{1}{2}(a_{t,w}+a_{t+1,w})\\ V_{t+1}=V_t+a_{t,w}^{'}\Delta t\\ P_{t+1}=P_t+ V_t\Delta t + \frac{1}{2}a_{t,w}^{'}\Delta t ^2 \]
具體代碼見VINS_MONO的predict()函數。